Diff: main.cpp

Revision:

30:c25c5bf0d951

Parent:

29:50e6e4e46580

Child:

31:ebe42589ab9d

--- a/main.cpp	Mon Nov 07 11:22:25 2016 +0000
+++ b/main.cpp	Mon Nov 07 13:25:35 2016 +0000
@@ -6,6 +6,8 @@
 #include "PwmIn.h"
 #include "MathHelpers.h"
 
+#include "driving.h"
+
 #include "config_motor.h"
 #include "config_loop.h"
 #include "config_pins.h"
@@ -23,7 +25,7 @@
 
 int adval1, adval2;
 float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
-float p_mech, last_p_mech, w;
+float p_mech, last_p_mech, w = 0.0f;
 float d_filtered = 0.0f, q_filtered = 0.0f;
 
 float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV)
@@ -80,7 +82,11 @@
 }
 
 float get_torque_cmd(float throttle, float w) {
-    return throttle * FORWARD_TORQUE_MAX;
+    if (TORQUE_MODE) {
+        return throttle * FORWARD_TORQUE_MAX;
+    } else {
+        return get_driving_torque_cmd(throttle, w);
+    }
 }
 
 //fill in d, q ref based on torque cmd and current velocity
@@ -127,17 +133,17 @@
     q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
     d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
     
-    if(control_enabled) {
-        vd = KP * d_err + d_integral;
-        vq = KP * q_err + q_integral;
-    } else {
-        vd = 0;
-        vq = 0;
-    }
+    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);
     
+    if (!control_enabled) {
+        vd = 0.0f;
+        vq = 0.0f;
+    }
+    
     float valpha = vd * cos_p - vq * sin_p;
     float vbeta = vd * sin_p + vq * cos_p;