Revision 24:5e18a87a0e95, committed 2016-11-05

Comitter:

bwang

Date:

Sat Nov 05 14:02:22 2016 +0000

Parent:

23:c77d4b42de17

Child:

25:3f2b585ae72d

Commit message:

working velocity code, with caveats; fixed 50A iq

Changed in this revision

--- a/PositionSensor/PositionSensor.cpp	Sat Nov 05 11:10:37 2016 +0000
+++ b/PositionSensor/PositionSensor.cpp	Sat Nov 05 14:02:22 2016 +0000
@@ -57,11 +57,11 @@
     int raw = TIM2->CNT;
     if (raw < 0) raw += _cpr;
     if (raw >= _cpr) raw -= _cpr;
-    float signed_mech = fmod(6.28318530718f * (raw) / (float)_cpr + _offset, 6.28318530718f);
-    if (signed_mech < 0){
-        return signed_mech + 6.28318530718f;
+    float mp = fmod(1 / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset), 2 * PI);
+    if (mp < 0){
+        return mp + 2 * PI;
     } else {
-        return signed_mech;
+        return mp;
     }  
 }
 
@@ -73,18 +73,18 @@
     int raw = TIM2->CNT;
     if (raw < 0) raw += _cpr;
     if (raw >= _cpr) raw -= _cpr;
-    float signed_elec = fmod((POLE_PAIRS / RESOLVER_LOBES * (6.28318530718f * (raw) / (float)_cpr + _offset)), 6.28318530718f);
-    if (signed_elec < 0) {
-        return signed_elec + 6.28318530718f;
+    float ep = fmod((POLE_PAIRS / RESOLVER_LOBES * (2 * PI * (raw) / (float)_cpr + _offset)), 2 * PI);
+    if (ep < 0) {
+        return ep + 2 * PI;
     } else {
-        return signed_elec;
+        return ep;
     }
 }
 
 void PositionSensorEncoder::ZeroEncoderCount(void){
     if (ZSense->read() == 1){
         if (ZSense->read() == 1){
-            TIM2->CNT=0;
+            TIM2->CNT = 0;
         }
     }
 }
\ No newline at end of file

--- a/config_loop.h	Sat Nov 05 11:10:37 2016 +0000
+++ b/config_loop.h	Sat Nov 05 14:02:22 2016 +0000
@@ -11,7 +11,7 @@
 
 #define INTEGRAL_MAX 1.0f
 
-#define W_FILTER_STRENGTH 0.9f
+#define W_FILTER_STRENGTH 0.99f
 #define DQ_FILTER_STRENGTH 0.0f
 
 #endif
\ No newline at end of file

--- a/config_motor.h	Sat Nov 05 11:10:37 2016 +0000
+++ b/config_motor.h	Sat Nov 05 14:02:22 2016 +0000
@@ -2,7 +2,7 @@
 #define __CONFIG_MOTOR_H
 
 #define POLE_PAIRS 3.0f
-#define POS_OFFSET 1.49f
+#define POS_OFFSET 5.88f
 #define RESOLVER_LOBES 3.0f
 #define CPR 4096
 #define KT 0.3f //Nm/A -- filler value until the LUT is made

--- a/config_pins.h	Sat Nov 05 11:10:37 2016 +0000
+++ b/config_pins.h	Sat Nov 05 14:02:22 2016 +0000
@@ -1,8 +1,8 @@
 #ifndef __CONFIG_PINS_H
 #define __CONFIG_PINS_H
 
-#define PWMA PA_9
-#define PWMB PA_8
+#define PWMA PA_8
+#define PWMB PA_9
 #define PWMC PA_10
 #define EN PB_15
 
@@ -11,8 +11,8 @@
 
 #define TH_PIN PB_8
 
-#define CURRENT_U ib
-#define CURRENT_V ia
+#define CURRENT_U ia
+#define CURRENT_V ib
 
 #define I_SCALE_RAW 25.0f //mv/A
 #define R_UP 12000.0f //ohms

--- 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;