Diff: main.cpp

Revision:

19:a6cf15f89f3d

Parent:

18:3863ca45cf26

Child:

20:91ae97a811e3

--- a/main.cpp	Mon Oct 31 06:53:28 2016 +0000
+++ b/main.cpp	Wed Nov 02 12:52:00 2016 +0000
@@ -1,11 +1,14 @@
 #include "mbed.h"
 #include "math.h"
+
 #include "PositionSensor.h"
 #include "FastPWM.h"
 #include "PwmIn.h"
+#include "MathHelpers.h"
 
 #include "config_motor.h"
 #include "config_loop.h"
+#include "config_pins.h"
 #include "config_inverter.h"
 
 FastPWM *a;
@@ -36,8 +39,6 @@
 
 void go_enabled();
 void go_disabled();
-float fminf(float, float);
-float fmaxf(float, float);
 
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     if (TIM1->SR & TIM_SR_UIF ) {
@@ -68,16 +69,15 @@
     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;
+    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
     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;
+    return throttle * FORWARD_TORQUE_MAX;
 }
 
 //fill in d, q ref based on torque cmd and current velocity
@@ -96,7 +96,7 @@
     if (p < 0) p += 2 * PI;
     
     float torque = get_torque_cmd(throttle_in.get_throttle(), w);
-    get_dq(torque * TORQUE_MAX, w, &d_ref, &q_ref);
+    get_dq(torque, w, &d_ref, &q_ref);
     
     float sin_p = sinf(p);
     float cos_p = cosf(p);
@@ -186,16 +186,6 @@
     en = 0;
 }
 
-float fminf(float a, float b) {
-    if(a < b) return a;
-    return b;
-}
-
-float fmaxf(float a, float b) {
-    if(a > b) return a;
-    return b;
-}
-
 void startup_msg() {
     pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
     pc.printf("%s\n\r", "====Config Data====");