Diff: main.cpp

Revision:

36:cac9785c91cb

Parent:

35:89385f64c867

Child:

37:ba7ebf4f8a78

--- a/main.cpp	Wed Nov 23 02:40:31 2016 +0000
+++ b/main.cpp	Sun Nov 27 09:10:09 2016 +0000
@@ -92,10 +92,13 @@
 
 //fill in d, q ref based on torque cmd and current velocity
 void get_dq(float torque, float w, float *d, float *q) {
+    /*
     *d = 0.0f;
     *q = torque / KT;
     if (*q > IPHASE_MOTOR_LIMIT) *q = IPHASE_MOTOR_LIMIT;
     if (*q > IPHASE_INVERTER_LIMIT) *q = IPHASE_INVERTER_LIMIT;
+    */
+    
     /*
     torque = fabsf(torque);
     get_mtpa_dq(torque, d, q);
@@ -103,7 +106,11 @@
         *d = -*d;
         *q = -*q;
     }
-    */    
+    */
+    
+    float is = torque / KT;
+    *d = (-FLUX_LINKAGE + sqrtf(FLUX_LINKAGE * FLUX_LINKAGE + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq));
+    *q = sqrtf(is * is - *d * *d);
 }
 
 void commutate() {
@@ -145,16 +152,16 @@
     vd = KP * d_err + d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE;
     vq = KP * q_err + q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE;
     
-    //vd = constrain(vd, -1.0f, 1.0f);
-    //vq = constrain(vq, -1.0f, 1.0f);
+    vd = constrain(vd, -1.0f, 1.0f);
+    vq = constrain(vq, -1.0f, 1.0f);
     
     float v = sqrtf(vd * vd + vq * vq);
     
+    /*
     if (v > 1.0f) {
-        //vd /= v;
-        //vq /= v;
         vq = sqrtf(1.0f - vd * vd);
     }
+    */
     
     if (!control_enabled) {
         vd = 0.0f;