Bayley Wang
/
foc-ed_in_the_bot_compact
robot
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====");