Diff: Calibration/Calibration.cpp

Revision:

119:ad7a6af6fba3

Parent:

118:2b6dab10b69d

Child:

157:a9b2002994d5

--- a/Calibration/Calibration.cpp	Mon Apr 24 01:51:16 2017 +0000
+++ b/Calibration/Calibration.cpp	Tue Apr 25 04:49:46 2017 +0000
@@ -30,16 +30,18 @@
 void calibrate_position(IOStruct *io) {
     io->pc->printf("%s\n", "Starting calibration procedure");
     
-    const int n = (int) (128 * POLE_PAIRS);
+    const int n = (int) (128);
     const int n2 = 10;
     
-    float delta = 2 * PI * POLE_PAIRS / (n * n2);
+    float delta = 2 * PI / (n * n2);
     
     float error_f[n] = {0};
     float error_b[n] = {0};
     
     float theta_ref = 0.0f;
     float theta_actual = 0.0f;
+    float theta_last = 0.0f;
+    float rollover = 0.0f;
     
     float vd = 0.5f;
     float vq = 0.0f;
@@ -54,6 +56,7 @@
         wait_us(100);
     }
     
+    theta_last = io->pos->GetElecPosition();
     for (int i = 0; i < n; i++) {
         for (int j = 0; j < n2; j++) {
             theta_ref += delta;
@@ -64,8 +67,17 @@
             set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
             wait_us(3000);
         }
-        theta_actual = io->pos->GetMechPosition();
-        error_f[i] = theta_ref / POLE_PAIRS - theta_actual;
+        
+        theta_actual = io->pos->GetElecPosition();
+        
+        //compensate for position rollover
+        if (theta_actual - theta_last < -PI) rollover += 2 * PI;
+        if (theta_actual - theta_last > PI) rollover -= 2 * PI;
+        io->pc->printf("%f,%f\n", theta_actual + rollover, theta_ref);
+        
+        theta_last = theta_actual;
+        
+        error_f[i] = theta_ref - theta_actual - rollover;
     }
     
     for (int i = 0; i < n; i++) {
@@ -78,14 +90,22 @@
             set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
             wait_us(3000);
         }
-        theta_actual = io->pos->GetMechPosition();
-        error_b[i] = theta_ref / POLE_PAIRS - theta_actual;
+        
+        theta_actual = io->pos->GetElecPosition();
+        
+        if (theta_actual - theta_last < -PI) rollover += 2 * PI;
+        if (theta_actual - theta_last > PI) rollover -= 2 * PI;
+        io->pc->printf("%f,%f\n", theta_actual + rollover, theta_ref);
+        
+        theta_last = theta_actual;
+        
+        error_b[i] = theta_ref - theta_actual- rollover;
     }
     
     float offset = 0.0f;
     for (int i = 0; i < n; i++) {
         offset += (error_f[i] + error_b[n - 1 - i]) / (2.0f * n);
     }
-    offset = fmodf(offset * POLE_PAIRS, 2 * PI);
+    offset = fmodf(offset, 2 * PI);
     io->pc->printf("Offset: %f\n", offset);
 }
\ No newline at end of file