Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 16:f283d6032fe5
- Parent:
- 15:b583cd30b063
- Child:
- 17:2b852039bb05
--- a/main.cpp Sun Oct 30 22:16:30 2016 +0000 +++ b/main.cpp Sun Oct 30 22:41:00 2016 +0000 @@ -2,7 +2,6 @@ #include "math.h" #include "PositionSensor.h" #include "FastPWM.h" -#include "Transforms.h" #include "config_motor.h" #include "config_loop.h" #include "config_inverter.h" @@ -12,14 +11,11 @@ FastPWM *b; FastPWM *c; DigitalOut en(EN); -DigitalOut toggle(PC_10); -PWM_IN p_in(TH_PIN, 1100, 1900); -bool control_enabled = false; +PwmIn throttle(TH_PIN, 1100, 1900); PositionSensorEncoder pos(CPR, 0); Serial pc(USBTX, USBRX); -int state = 0; int adval1, adval2; float ia, ib, ic, alpha, beta, d, q, vd, vq, p; float p_mech, last_p_mech, w; @@ -30,45 +26,23 @@ float last_d = 0.0f, last_q = 0.0f; float d_ref = 0.0f, q_ref = 0.0f; +bool control_enabled = false; 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; -} +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 ) { - toggle = 1; ADC1->CR2 |= 0x40000000; volatile int delay; for (delay = 0; delay < 35; delay++); - toggle = 0; adval1 = ADC1->DR; adval2 = ADC2->DR; commutate(); @@ -177,9 +151,8 @@ } 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; + if(control_enabled && !throttle.get_enabled()) go_disabled(); + if(!control_enabled && throttle.get_enabled()) go_enabled(); p = pos.GetElecPosition() - POS_OFFSET; if (p < 0) p += 2 * PI; @@ -193,6 +166,9 @@ float w_raw = dp_mech * (float) 1e6 / loop_period; //rad/s w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw; + q_ref = throttle.get_throttle() * Q_MAX; + d_ref = 0.0f; + float sin_p = sinf(p); float cos_p = cosf(p); @@ -268,3 +244,25 @@ //wait(0.1); } } + +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; +}