fix(calib): error in maxWingPos

controller/calibration.ino

@@ -17,6 +17,7 @@
 
 void Calibrate() {
   String input;
+  uint32_t calibTime = 6000;  // Calibration time in ms
 
   Serial.println("[INPUT]: Calibrate angles? (Y/n)");
 
@@ -28,7 +29,9 @@ void Calibrate() {
 
   } else {
 
-    Serial.println("[WARNING]: Setup will run for 8s to get the offset and maximum value.");
+    Serial.print("[WARNING]: Setup will run for ");
+    Serial.print(calibTime / 1000);
+    Serial.print("s to get the offset and maximum value.");
     Serial.println("[INPUT]: Type 'Y' to begin.");
 
     while (Serial.available() == 0) {}  // Wait for serial input
@@ -39,8 +42,8 @@ void Calibrate() {
       gEsc1.write(40);
       gEsc2.write(40);
 
-      GetAngleOffset();
-      GetMaxWingPos();
+      GetAngleOffset(calibTime / 2);
+      GetMaxWingPos(calibTime / 2);
 
       // Stop ESC
       gEsc1.write(0);
@@ -64,9 +67,7 @@ void Calibrate() {
 }
 
 // Determine if wing position needs to be offset to prevent wrap-around
-void GetAngleOffset() {
-
-  uint16_t calibTime = 4000;
+void GetAngleOffset(uint32_t calibTime) {
 
   unsigned int maxPos[4];
   unsigned int minPos[4];
@@ -99,12 +100,8 @@ void GetAngleOffset() {
 }
 
 
-unsigned int GetMaxWingPos() {
-
-  uint16_t calibTime = 4000;
+unsigned int GetMaxWingPos(uint32_t calibTime) {
 
-  unsigned int maxPos[4];
-  unsigned int minPos[4];
   unsigned int newPos;
 
   for (uint32_t tStart = millis(); (millis() - tStart) < calibTime;) {
@@ -112,7 +109,7 @@ unsigned int GetMaxWingPos() {
     // Find max and min wing position
     for (int i = 0; i < sizeof(gCSN_PINS); i++) {
       newPos = ReadSensor(gCSN_PINS[i], gENC_ORIENT[i], gAngleOffset[i]);
-      if (newPos > maxPos[i]) {
+      if (newPos > gMaxWingPos[i]) {
         gMaxWingPos[i] = newPos;
       }
     }