Diff: main.cpp

Revision:

18:3863ca45cf26

Parent:

17:2b852039bb05

Child:

19:a6cf15f89f3d

--- a/main.cpp	Sun Oct 30 22:46:49 2016 +0000
+++ b/main.cpp	Mon Oct 31 06:53:28 2016 +0000
@@ -2,16 +2,17 @@
 #include "math.h"
 #include "PositionSensor.h"
 #include "FastPWM.h"
+#include "PwmIn.h"
+
 #include "config_motor.h"
 #include "config_loop.h"
 #include "config_inverter.h"
-#include "pwm_in.h"
 
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
-PwmIn throttle(TH_PIN, 1100, 1900);
+PwmIn throttle_in(TH_PIN, 1100, 1900);
 PositionSensorEncoder pos(CPR, 0);
 
 Serial pc(USBTX, USBRX);
@@ -72,19 +73,30 @@
     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;
-}    
+}
+
+//torque command, in percent max torque
+float get_torque_cmd(float throttle, float w) {
+    return throttle;
+}
+
+//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;
+}
 
 void commutate() {
-    if(control_enabled && !throttle.get_enabled()) go_disabled();
-    if(!control_enabled && throttle.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;
     
-    q_ref = throttle.get_throttle() * Q_MAX;
-    d_ref = 0.0f;
+    float torque = get_torque_cmd(throttle_in.get_throttle(), w);
+    get_dq(torque * TORQUE_MAX, w, &d_ref, &q_ref);
     
     float sin_p = sinf(p);
     float cos_p = cosf(p);