Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 18:3863ca45cf26
- Parent:
- 17:2b852039bb05
- Child:
- 19:a6cf15f89f3d
diff -r 2b852039bb05 -r 3863ca45cf26 main.cpp --- 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);