Keep trying to calibrate gyro until it is standing still

Author: Lauszus

Firmware/Balanduino/Balanduino.h

@@ -228,7 +228,7 @@ void testMotorSpeed(double *leftSpeed, double *rightSpeed, double leftScaler, do
 void calibrateAcc();
 void printValues();
 void setValues(char *input);
-void calibrateGyro();
+bool calibrateGyro();
 bool checkMinMax(int16_t *array, uint8_t length, int16_t maxDifference);
 
 #endif

Firmware/Balanduino/Balanduino.ino

@@ -235,8 +235,8 @@ void setup() {
   pitch = accAngle;
   gyroAngle = accAngle;
 
-  /* Find gyro zero value */
-  calibrateGyro();
+  /* Calibrate gyro zero value */
+  while (calibrateGyro()); // Run again if the robot is moved while calibrating
 
   LED::SetDirWrite();
 

Firmware/Balanduino/Tools.ino

@@ -408,7 +408,7 @@ void setValues(char *input) {
 }
 #endif // ENABLE_TOOLS or ENABLE_SPP
 
-void calibrateGyro() {
+bool calibrateGyro() {
   int16_t gyroXbuffer[25];
   for (uint8_t i = 0; i < 25; i++) {
     while (i2cRead(0x43, i2cBuffer, 2));
@@ -418,11 +418,12 @@ void calibrateGyro() {
   if (!checkMinMax(gyroXbuffer, 25, 2000)) {
     Serial.print(F("Gyro calibration error"));
     buzzer::Set();
-    while (1); // Halt
+    return 1;
   }
   for (uint8_t i = 0; i < 25; i++)
     gyroXzero += gyroXbuffer[i];
   gyroXzero /= 25;
+  return 0;
 }
 
 bool checkMinMax(int16_t *array, uint8_t length, int16_t maxDifference) { // Used to check that the robot is laying still while calibrating