Diff: main.cpp

Revision:

22:72840d3db788

Parent:

21:b7fb355c8c2d

Child:

23:c77d4b42de17

--- a/main.cpp	Sat Nov 05 09:12:10 2016 +0000
+++ b/main.cpp	Sat Nov 05 10:50:25 2016 +0000
@@ -23,6 +23,7 @@
 int adval1, adval2;
 float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
 float p_mech, last_p_mech, w;
+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)
 
@@ -83,7 +84,7 @@
 //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 < Q_MAX ? torque / KT : Q_MAX;
+    *q = Q_MAX;//torque / KT < Q_MAX ? torque / KT : Q_MAX;
 }
 
 void commutate() {
@@ -114,8 +115,11 @@
     d = alpha * cos_p - beta * sin_p;
     q = -alpha * sin_p - beta * cos_p;
     
-    float d_err = d_ref - d;
-    float q_err = q_ref - q;
+    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;
+    
+    float d_err = d_ref - d_filtered;
+    float q_err = q_ref - q_filtered;
     
     d_integral += d_err * KI;
     q_integral += q_err * KI;
@@ -209,7 +213,7 @@
     TIM1->EGR |= TIM_EGR_UG;
     
     TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock
-    TIM1->ARR = 0x4650; //5 Khz
+    TIM1->ARR = (int) ((float) 9e7 / F_SW);
     TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
     TIM1->CR1 |= TIM_CR1_CEN;