Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 15:b583cd30b063
- Parent:
- 14:59c4fcc1a4f7
- Child:
- 16:f283d6032fe5
--- a/main.cpp Sun Oct 30 02:19:41 2016 +0000 +++ b/main.cpp Sun Oct 30 22:16:30 2016 +0000 @@ -3,7 +3,9 @@ #include "PositionSensor.h" #include "FastPWM.h" #include "Transforms.h" -#include "config.h" +#include "config_motor.h" +#include "config_loop.h" +#include "config_inverter.h" #include "pwm_in.h" FastPWM *a; @@ -11,7 +13,7 @@ FastPWM *c; DigitalOut en(EN); DigitalOut toggle(PC_10); -PWM_IN p_in(PB_8, 1100, 1900); +PWM_IN p_in(TH_PIN, 1100, 1900); bool control_enabled = false; PositionSensorEncoder pos(CPR, 0); @@ -20,6 +22,7 @@ int state = 0; int adval1, adval2; float ia, ib, ic, alpha, beta, d, q, vd, vq, p; +float p_mech, last_p_mech, w; float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV) @@ -154,6 +157,7 @@ wait_ms(250); zero_current(); + p_mech = pos.GetMechPosition(); en = 1; } @@ -176,9 +180,19 @@ if(control_enabled && !p_in.get_enabled()) go_disabled(); if(!control_enabled && p_in.get_enabled()) go_enabled(); q_ref = p_in.get_throttle() * Q_MAX; + p = pos.GetElecPosition() - POS_OFFSET; if (p < 0) p += 2 * PI; + 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; + 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; + float sin_p = sinf(p); float cos_p = cosf(p); @@ -222,7 +236,7 @@ if (vq < -1.0f) vq = -1.0f; if (vq > 1.0f) vq = 1.0f; - DAC->DHR12R2 = (unsigned int) (-q * 20 + 2048); //uncomment me to write I_q to the DAC + //DAC->DHR12R2 = (unsigned int) (-q * 20 + 2048); //uncomment me to write I_q to the DAC //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048); //vd = 0.0f; //uncomment me for voltage mode testing