Diff: Calibration/Calibration.cpp

Revision:

117:97da9eb4300e

Child:

118:2b6dab10b69d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/Calibration.cpp	Sun Apr 23 07:17:48 2017 +0000
@@ -0,0 +1,91 @@
+#include <math.h>
+
+#include "BREMSStructs.h"
+#include "Calibration.h"
+#include "MathHelpers.h"
+#include "Transforms.h"
+#include "config_pins.h"
+#include "config_motor.h"
+#include "config_inverter.h"
+
+//output is in modulation depth
+void abc(float theta, float vd, float vq, float *a, float *b, float *c) {
+    float valpha, vbeta;
+    float va, vb, vc, voff;
+    
+    invpark(vd, vq, sinf(theta), cosf(theta), &valpha, &vbeta);
+    invclarke(valpha, vbeta, &va, &vb);
+    vc = -va - vb;
+    
+    voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
+    va = va - voff;
+    vb = vb - voff;
+    vc = vc - voff;
+    
+    *a = va;
+    *b = vb;
+    *c = vc;
+}
+
+void calibrate_position(IOStruct *io, ControlStruct *control) {
+    io->pc->printf("%s\n", "Starting calibration procedure");
+    
+    const int n = (int) (128 * POLE_PAIRS);
+    const int n2 = 10;
+    
+    float delta = 2 * PI * POLE_PAIRS / (n * n2);
+    
+    float error_f[n] = {0};
+    float error_b[n] = {0};
+    
+    float theta_ref = 0.0f;
+    float theta_actual = 0.0f;
+    
+    float vd = 0.1f;
+    float vq = 0.0f;
+    float va, vb, vc = 0.0f;
+    
+    abc(theta_ref, vd, vq, &va, &vb, &vc);
+    
+    for (int i = 0; i < 40000; i++) {
+        set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
+        set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
+        set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
+        wait_us(100);
+    }
+    
+    for (int i = 0; i < n; i++) {
+        for (int j = 0; j < n2; j++) {
+            theta_ref += delta;
+            abc(theta_ref, vd, vq, &va, &vb, &vc);
+            
+            set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
+            set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
+            set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
+            wait_us(100);
+        }
+        theta_actual = io->pos->GetMechPosition();
+        error_f[i] = theta_ref / POLE_PAIRS - theta_actual;
+    }
+    
+    for (int i = 0; i < n; i++) {
+        for (int j = 0; j < n2; j++) {
+            theta_ref -= delta;
+            abc(theta_ref, vd, vq, &va, &vb, &vc);
+            
+            set_dtc(io->a, 0.5f + 0.5f * va * MODULATION_MAX);
+            set_dtc(io->b, 0.5f + 0.5f * vb * MODULATION_MAX);
+            set_dtc(io->c, 0.5f + 0.5f * vc * MODULATION_MAX);
+            wait_us(100);
+        }
+        theta_actual = io->pos->GetMechPosition();
+        error_b[i] = theta_ref / POLE_PAIRS - theta_actual;
+    }
+    
+    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);
+    io->pc->printf("Offset: %f\n", offset);
+}
\ No newline at end of file