Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Diff: main.cpp
- Revision:
- 42:030e0ec4eac5
- Parent:
- 40:22aede3d096f
- Child:
- 44:3fd6a43b91f0
--- a/main.cpp Thu Jan 05 05:54:20 2017 +0000 +++ b/main.cpp Fri Jan 06 07:54:22 2017 +0000 @@ -5,8 +5,12 @@ #include "FastPWM.h" #include "PwmIn.h" #include "MathHelpers.h" -#include "Driving.h" -#include "LutOptimize.h" +#include "Transforms.h" +#include "DQMapper.h" +#include "ThrottleMapper.h" + +#include "BREMSStructs.h" +#include "BREMSConfig.h" #include "config_motor.h" #include "config_loop.h" @@ -14,258 +18,131 @@ #include "config_inverter.h" #include "config_driving.h" -FastPWM *a; -FastPWM *b; -FastPWM *c; -DigitalOut en(EN); -PwmIn throttle_in(TH_PIN, 1100, 1900); -PositionSensorEncoder pos(CPR, 0); - -Serial pc(USBTX, USBRX); +#include "main.h" -int adval1, adval2, adval3; -float vbus = BUS_VOLTAGE; -float ia, ib, ic, alpha, beta, d, q, vd, vq, p; -float p_mech, last_p_mech, w = 0.0f; -float d_filtered = 0.0f, q_filtered = 0.0f; +IOStruct io; +ReadDataStruct read; +FOCStruct foc; +ControlStruct control; -float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV) - -float d_integral = 0.0f, q_integral = 0.0f; -float last_d = 0.0f, last_q = 0.0f; -float d_ref = 0.0f, q_ref = 0.0f; +DQMapper *dq; +ThrottleMapper *th; bool control_enabled = false; -void commutate(); -void zero_current(); -void config_globals(); -void startup_msg(); +void update_velocity() { + read.last_p_mech = read.p_mech; + read.p_mech = io.pos->GetMechPosition(); + float dp_mech = read.p_mech - read.last_p_mech; + if (dp_mech < -PI) dp_mech += 2 * PI; + if (dp_mech > PI) dp_mech -= 2 * PI; + float w_raw = dp_mech * F_SW; //rad/s + if (w_raw > W_CRAZY) w_raw = read.w; //with this limiting scheme noise < 0 + if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first + read.w = update_pid(read.w, w_raw, W_FILTER_STRENGTH); +} -void go_enabled(); -void go_disabled(); +void commutate() { + /*safety checks*/ + if (control_enabled && !io.throttle_in->get_enabled()) go_disabled(); + if (control_enabled && !io.pos->IsValid()) go_disabled(); + if (!control_enabled && io.throttle_in->get_enabled()) go_enabled(); + + /*update velocity, references*/ + update_velocity(); + float torque_percent = th->map(io.throttle_in->get_throttle(), read.w); + dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref); + + /*update position, sin, cos*/ + foc.p = io.pos->GetElecPosition() - POS_OFFSET; + float sin_p = sinf(foc.p); + float cos_p = cosf(foc.p); + + /*scale and offset currents (adval1, 2 are updated in ISR*/ + foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ia_supp_offset) / I_SCALE; + foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ib_supp_offset) / I_SCALE; + + /*compute d, q*/ + clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta); + park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q); + + /*PI controller*/ + control.d_filtered = update_pid(control.d_filtered, foc.d, DQ_FILTER_STRENGTH); + control.q_filtered = update_pid(control.q_filtered, foc.q, DQ_FILTER_STRENGTH); + + float d_err = control.d_ref - control.d_filtered; + float q_err = control.q_ref - control.q_filtered; + + control.d_integral += d_err * KI; + control.q_integral += q_err * KI; + + control.q_integral = constrain(control.q_integral, -INTEGRAL_MAX, INTEGRAL_MAX); + control.d_integral = constrain(control.d_integral, -INTEGRAL_MAX, INTEGRAL_MAX); + + foc.vd = KP * d_err + control.d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE; + foc.vq = KP * q_err + control.q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE; + + foc.vd = constrain(foc.vd, -1.0f, 1.0f); + foc.vq = constrain(foc.vq, -1.0f, 1.0f); + + if (!control_enabled) { + foc.vd = 0.0f; + foc.vq = 0.0f; + } + + /*inverse transforms*/ + invpark(foc.vd, foc.vq, sin_p, cos_p, &foc.valpha, &foc.vbeta); + + float va, vb, vc, voff; + + invclarke(foc.valpha, foc.vbeta, &va, &vb); + vc = -va - vb; + + /*SVPWM*/ + voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f; + va = va - voff; + vb = vb - voff; + vc = vc - voff; + + /*output to timers*/ + set_dtc(io.a, 0.5f + 0.5f * va); + set_dtc(io.b, 0.5f + 0.5f * vb); + set_dtc(io.c, 0.5f + 0.5f * vc); +} + +void go_enabled() { + control.d_integral = 0.0f; + control.q_integral = 0.0f; + control_enabled = true; + io.en->write(1); +} + +void go_disabled() { + control_enabled = false; + io.en->write(0); +} + +float update_pid(float old, float x, float str) { + return str * old + (1.0f - str) * x; +} extern "C" void TIM1_UP_TIM10_IRQHandler(void) { if (TIM1->SR & TIM_SR_UIF ) { ADC1->CR2 |= 0x40000000; volatile int delay; for (delay = 0; delay < 35; delay++); - adval1 = ADC1->DR; - adval2 = ADC2->DR; + read.adval1 = ADC1->DR; + read.adval2 = ADC2->DR; commutate(); } TIM1->SR = 0x00; } -void zero_current(){ - for (int i = 0; i < 1000; i++){ - ia_supp_offset += (float) (ADC1->DR); - ib_supp_offset += (float) (ADC2->DR); - ADC1->CR2 |= 0x40000000; - wait_us(100); - } - ia_supp_offset /= 1000.0f; - ib_supp_offset /= 1000.0f; - ia_supp_offset = ia_supp_offset / 4096.0f * AVDD - I_OFFSET; - ib_supp_offset = ib_supp_offset / 4096.0f * AVDD - I_OFFSET; -} - -void update_velocity() { - last_p_mech = p_mech; - p_mech = pos.GetMechPosition(); - float dp_mech = p_mech - last_p_mech; - if (dp_mech < -PI) dp_mech += 2 * PI; - if (dp_mech > PI) dp_mech -= 2 * PI; - float w_raw = dp_mech * F_SW; //rad/s - if (w_raw > W_CRAZY) w_raw = w; //with this limiting scheme noise < 0 - if (w_raw < -W_CRAZY) w_raw = w; //so we need to throw out the large deltas first - w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw; -} - -/*map throttle to percent max torque*/ -float get_tqpct_cmd(float throttle, float w) { - float tq; - if (TORQUE_MODE) { - tq = throttle; - } else { - tq = get_driving_tqpct_cmd(throttle, w); - } - return tq; -} - -/*get d, q based on % max torque command and velocity*/ -void get_dq(float torque_percent, float w, float *d, float *q) { - get_dq_lut(torque_percent, w, d, q); -} - -void commutate() { - if (control_enabled && !throttle_in.get_enabled()) go_disabled(); - if (control_enabled && !pos.IsValid()) go_disabled(); - if (!control_enabled && throttle_in.get_enabled()) go_enabled(); - - update_velocity(); - - p = pos.GetElecPosition() - POS_OFFSET; - float sin_p = sinf(p); - float cos_p = cosf(p); - - float torque_percent = get_tqpct_cmd(throttle_in.get_throttle(), w); - - ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE; - ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE; - - alpha = ia; - beta = 1 / sqrtf(3.0f) * ia + 2 / sqrtf(3.0f) * ib; - - d = alpha * cos_p + beta * sin_p; - q = -alpha * sin_p + beta * cos_p; - - d_filtered = DQ_FILTER_STRENGTH * d_filtered + (1.0f - DQ_FILTER_STRENGTH) * d; - q_filtered = DQ_FILTER_STRENGTH * q_filtered + (1.0f - DQ_FILTER_STRENGTH) * q; - - get_dq(torque_percent, w, &d_ref, &q_ref); - - float d_err = d_ref - d_filtered; - float q_err = q_ref - q_filtered; - - d_integral += d_err * KI; - q_integral += q_err * KI; - - q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX); - d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX); - - vd = KP * d_err + d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE; - vq = KP * q_err + q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE; - - vd = constrain(vd, -1.0f, 1.0f); - vq = constrain(vq, -1.0f, 1.0f); - - if (!control_enabled) { - vd = 0.0f; - vq = 0.0f; - } - - float valpha = vd * cos_p - vq * sin_p; - float vbeta = vd * sin_p + vq * cos_p; - - float va = valpha; - 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); -} - int main() { - config_globals(); - startup_msg(); + BREMSInit(&io, &read, &foc, &control, false); + dq = new LutMapper(); + th = new DrivingThrottleMapper(); for (;;) { } -} - -void go_enabled() { - d_integral = 0.0f; - q_integral = 0.0f; - control_enabled = true; - en = 1; -} - -void go_disabled() { - control_enabled = false; - en = 0; -} - -void startup_msg() { - pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A."); - pc.printf("%s\n\r", "====Config Data===="); - pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET); - pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE); - pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE); - pc.printf("Switching Frequency: %f KHz \n\r", F_SW / 1000.0f); - pc.printf("Pole pairs: %d\n\r", (int) POLE_PAIRS); - pc.printf("Resolver lobes: %d\n\r", (int) RESOLVER_LOBES); - pc.printf("Loop KP: %f\n\r", KP); - pc.printf("Loop KI: %f\n\r", KI); - pc.printf("Ia offset: %f mV\n\r", ia_supp_offset); - pc.printf("Ib offset: %f mV\n\r", ib_supp_offset); - pc.printf("\n\r"); -} - -void config_globals() { - pc.baud(115200); - - //Enable clocks for GPIOs - RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; - RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; - RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; - - RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock - - a = new FastPWM(PWMA); - b = new FastPWM(PWMB); - c = new FastPWM(PWMC); - - NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ - - TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt - TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up - TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on, - TIM1->RCR |= 0x01; //update event once per up/down count of tim1 - TIM1->EGR |= TIM_EGR_UG; - - TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock - TIM1->ARR = (int) ((float) 9e7 / F_SW); - TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on. - TIM1->CR1 |= TIM_CR1_CEN; - - //ADC Setup - RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1 - RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2 - - ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels - - ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on - ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0 - - ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON - ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1 - - GPIOA->MODER |= (1 << 8); - GPIOA->MODER |= (1 << 9); - - GPIOA->MODER |= (1 << 2); - GPIOA->MODER |= (1 << 3); - - GPIOA->MODER |= (1 << 0); - GPIOA->MODER |= (1 << 1); - - GPIOB->MODER |= (1 << 0); - GPIOB->MODER |= (1 << 1); - - GPIOC->MODER |= (1 << 2); - GPIOC->MODER |= (1 << 3); - - //DAC setup - RCC->APB1ENR |= 0x20000000; - DAC->CR |= DAC_CR_EN2; - - GPIOA->MODER |= (1 << 10); - GPIOA->MODER |= (1 << 11); - - //Zero duty cycles - set_dtc(a, 0.0f); - set_dtc(b, 0.0f); - set_dtc(c, 0.0f); - - wait_ms(250); - zero_current(); - p_mech = pos.GetMechPosition(); - en = 1; -} +} \ No newline at end of file