Diff: main.cpp

Revision:

39:80b38a8e1787

Parent:

38:07cb4ae6c1bd

Child:

40:22aede3d096f

--- a/main.cpp	Tue Nov 29 00:31:18 2016 +0000
+++ b/main.cpp	Sat Dec 03 17:49:02 2016 +0000
@@ -83,17 +83,29 @@
 }
 
 float get_torque_cmd(float throttle, float w) {
+    float tq;
     if (TORQUE_MODE) {
-        return throttle * FORWARD_TORQUE_MAX;
+        tq =  throttle * FORWARD_TORQUE_MAX;
     } else {
-        return get_driving_torque_cmd(throttle, w);
+        tq =  get_driving_torque_cmd(throttle, w);
     }
+    return tq;
 }
 
 //fill in d, q ref based on torque cmd and current velocity
 void get_dq(float torque, float w, float *d, float *q) { 
-    torque = fabsf(torque);
-    get_mtpa_dq(torque, d, q);
+    float tqabs = fabsf(torque);
+    
+    /*get voltage limits and estimate current voltage*/
+    float vs_max2 = (BUS_VOLTAGE * MODULATION_INDEX / 2.0f) * (BUS_VOLTAGE * MODULATION_INDEX / 2.0f);
+    float vs2 = get_vs2(w, d_filtered, q_filtered);
+    
+    if (vs2 < vs_max2) {
+        get_mtpa_dq(tqabs, d, q);
+    } else {
+        get_mtpf_dq(tqabs, w, vs_max2, d, q);
+    }
+    
     if (torque < 0.0f) {
         *d = -*d;
         *q = -*q;
@@ -108,12 +120,10 @@
     update_velocity();
     
     p = pos.GetElecPosition() - POS_OFFSET;
+    float sin_p = sinf(p);
+    float cos_p = cosf(p);
     
     float torque = get_torque_cmd(throttle_in.get_throttle(), w);
-    get_dq(torque, w, &d_ref, &q_ref);
-    
-    float sin_p = sinf(p);
-    float cos_p = cosf(p);
     
     ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE;
     ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE;
@@ -127,6 +137,8 @@
     d_filtered = DQ_FILTER_STRENGTH * d_filtered + (1.0f - DQ_FILTER_STRENGTH) * d;
     q_filtered = DQ_FILTER_STRENGTH * q_filtered + (1.0f - DQ_FILTER_STRENGTH) * q;
     
+    get_dq(torque, w, &d_ref, &q_ref);
+    
     float d_err = d_ref - d_filtered;
     float q_err = q_ref - q_filtered;
     
@@ -142,14 +154,6 @@
     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) {
-        vq = sqrtf(1.0f - vd * vd);
-    }
-    */
-    
     if (!control_enabled) {
         vd = 0.0f;
         vq = 0.0f;