Diff: main.cpp

Revision:

15:b583cd30b063

Parent:

14:59c4fcc1a4f7

Child:

16:f283d6032fe5

--- a/main.cpp	Sun Oct 30 02:19:41 2016 +0000
+++ b/main.cpp	Sun Oct 30 22:16:30 2016 +0000
@@ -3,7 +3,9 @@
 #include "PositionSensor.h"
 #include "FastPWM.h"
 #include "Transforms.h"
-#include "config.h"
+#include "config_motor.h"
+#include "config_loop.h"
+#include "config_inverter.h"
 #include "pwm_in.h"
 
 FastPWM *a;
@@ -11,7 +13,7 @@
 FastPWM *c;
 DigitalOut en(EN);
 DigitalOut toggle(PC_10);
-PWM_IN p_in(PB_8, 1100, 1900);
+PWM_IN p_in(TH_PIN, 1100, 1900);
 bool control_enabled = false;
 PositionSensorEncoder pos(CPR, 0);
 
@@ -20,6 +22,7 @@
 int state = 0;
 int adval1, adval2;
 float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
+float p_mech, last_p_mech, w;
 
 float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV)
 
@@ -154,6 +157,7 @@
     
     wait_ms(250);
     zero_current();
+    p_mech = pos.GetMechPosition();
     en = 1;
 }
 
@@ -176,9 +180,19 @@
     if(control_enabled && !p_in.get_enabled()) go_disabled();
     if(!control_enabled && p_in.get_enabled()) go_enabled();
     q_ref = p_in.get_throttle() * Q_MAX;
+    
     p = pos.GetElecPosition() - POS_OFFSET;
     if (p < 0) p += 2 * PI;
     
+    last_p_mech = p_mech;
+    p_mech = pos.GetMechPosition();
+    float dp_mech = p_mech - last_p_mech;
+    if (dp_mech < 0.0f) dp_mech += 2 * PI;
+    if (dp_mech > 2 * PI) dp_mech -= 2 * PI;
+    float loop_period = (float) (TIM1->ARR) / 90.0f;
+    float w_raw = dp_mech * (float) 1e6 / loop_period; //rad/s
+    w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw;
+    
     float sin_p = sinf(p);
     float cos_p = cosf(p);
     
@@ -222,7 +236,7 @@
     if (vq < -1.0f) vq = -1.0f;
     if (vq > 1.0f) vq = 1.0f;
     
-    DAC->DHR12R2 = (unsigned int) (-q * 20 + 2048); //uncomment me to write I_q to the DAC
+    //DAC->DHR12R2 = (unsigned int) (-q * 20 + 2048); //uncomment me to write I_q to the DAC
     //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048);
     
     //vd = 0.0f; //uncomment me for voltage mode testing