Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 13:41d102a53caf
- Parent:
- 12:5723a4fa5864
- Child:
- 14:59c4fcc1a4f7
--- a/main.cpp Sun Oct 30 01:37:30 2016 +0000 +++ b/main.cpp Sun Oct 30 02:06:03 2016 +0000 @@ -4,13 +4,15 @@ #include "FastPWM.h" #include "Transforms.h" #include "config.h" +#include "pwm_in.h" FastPWM *a; FastPWM *b; FastPWM *c; DigitalOut en(EN); DigitalOut toggle(PC_10); - +PWM_IN p_in(PB_8, 1100, 1900); +bool control_enabled = false; PositionSensorEncoder pos(CPR, 0); Serial pc(USBTX, USBRX); @@ -25,11 +27,38 @@ float last_d = 0.0f, last_q = 0.0f; float d_ref = 0.0f, q_ref = 50.0f; + void commutate(); void zero_current(); void config_globals(); void startup_msg(); +void go_enabled() +{ + d_integral = 0.0f; + q_integral = 0.0f; + control_enabled = true; + en = 1; +} + +void go_disabled() +{ + control_enabled = false; + 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; +} + extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { toggle = 1; @@ -144,6 +173,9 @@ } void commutate() { + 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; @@ -177,8 +209,16 @@ if (q_integral < -INTEGRAL_MAX) q_integral = -INTEGRAL_MAX; if (d_integral < -INTEGRAL_MAX) d_integral = -INTEGRAL_MAX; - vd = KP * d_err + d_integral; - vq = KP * q_err + q_integral; + if(control_enabled) + { + vd = KP * d_err + d_integral; + vq = KP * q_err + q_integral; + } + else + { + vd = 0; + vq = 0; + } if (vd < -1.0f) vd = -1.0f; if (vd > 1.0f) vd = 1.0f; @@ -198,6 +238,11 @@ float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta; float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta; + float voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f; + va = va - voff; + vb = vb - voff; + vc = vc - voff; + set_dtc(a, 0.5f + 0.5f * va); set_dtc(b, 0.5f + 0.5f * vb); set_dtc(c, 0.5f + 0.5f * vc);