Diff: main.cpp

Revision:

31:ebe42589ab9d

Parent:

30:c25c5bf0d951

Child:

32:b31423041c4e

--- a/main.cpp	Mon Nov 07 13:25:35 2016 +0000
+++ b/main.cpp	Thu Nov 10 14:43:25 2016 +0000
@@ -5,8 +5,8 @@
 #include "FastPWM.h"
 #include "PwmIn.h"
 #include "MathHelpers.h"
-
-#include "driving.h"
+#include "Driving.h"
+#include "Optimize.h"
 
 #include "config_motor.h"
 #include "config_loop.h"
@@ -95,6 +95,14 @@
     *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_ipm_dq(torque, d, q);
+    if (torque < 0.0f) {
+        *d = -*d;
+        *q = -*q;
+    }
+    */    
 }
 
 void commutate() {
@@ -133,11 +141,18 @@
     q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
     d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
     
-    vd = KP * d_err + d_integral - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE * (float) (1e-6);
-    vq = KP * q_err + q_integral + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE * (float) (1e-6);
+    vd = KP * d_err + d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE * (float) (1e-6);
+    vq = KP * q_err + q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE * (float) (1e-6);
+    
+    //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;
+    }
     
     if (!control_enabled) {
         vd = 0.0f;