Diff: main.cpp

Revision:

24:5e18a87a0e95

Parent:

23:c77d4b42de17

Child:

25:3f2b585ae72d

--- a/main.cpp	Sat Nov 05 11:10:37 2016 +0000
+++ b/main.cpp	Sat Nov 05 14:02:22 2016 +0000
@@ -31,7 +31,7 @@
 float last_d = 0.0f, last_q = 0.0f;
 float d_ref = 0.0f, q_ref = 0.0f;
 
-bool control_enabled = false;
+bool control_enabled = true;
 
 void commutate();
 void zero_current();
@@ -71,9 +71,10 @@
     p_mech = pos.GetMechPosition();
     float dp_mech = p_mech - last_p_mech;
     if (dp_mech < -PI) dp_mech += 2 * PI;
-    if (dp_mech > PI) dp_mech -= 2 * PI;
-    float loop_period = (float) (TIM1->ARR) / 90.0f;
-    float w_raw = dp_mech * (float) 1e6 / loop_period; //rad/s
+    if (dp_mech >  PI) dp_mech -= 2 * PI;
+    float w_raw = dp_mech * F_SW; //rad/s
+    if (w_raw > W_MAX) w_raw = w; //with this limiting scheme noise < 0
+    if (w_raw < -W_MAX) w_raw = w; //so we need to throw out the large deltas first
     w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw;
 }
 
@@ -88,13 +89,12 @@
 }
 
 void commutate() {
-    if(control_enabled && !throttle_in.get_enabled()) go_disabled();
-    if(!control_enabled && throttle_in.get_enabled()) go_enabled();
+    //if(control_enabled && !throttle_in.get_enabled()) go_disabled();
+    //if(!control_enabled && throttle_in.get_enabled()) go_enabled();
     
     update_velocity();
     
     p = pos.GetElecPosition() - POS_OFFSET;
-    if (p < 0) p += 2 * PI;
     
     float torque = get_torque_cmd(throttle_in.get_throttle(), w);
     get_dq(torque, w, &d_ref, &q_ref);
@@ -112,8 +112,8 @@
     alpha = u;
     beta = 1 / sqrtf(3.0f) * u + 2 / sqrtf(3.0f) * v;
     
-    d = alpha * cos_p - beta * sin_p;
-    q = -alpha * sin_p - beta * cos_p;
+    d = alpha * cos_p + beta * sin_p;
+    q = -alpha * sin_p + beta * cos_p;
     
     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;
@@ -142,8 +142,8 @@
     float vbeta = vd * sin_p + vq * cos_p;
     
     float va = valpha;
-    float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
-    float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
+    float vb = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
+    float vc = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
     
     float voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;
     va = va - voff;