Bayley Wang
/
foc-ed_in_the_bot_compact
robot
main.cpp
- Committer:
- bwang
- Date:
- 2017-01-25
- Revision:
- 55:3568b78dc203
- Parent:
- 54:25db122c05f0
- Child:
- 56:c681001dfa46
File content as of revision 55:3568b78dc203:
#include "mbed.h" #include "math.h" #include "PositionSensor.h" #include "FastPWM.h" #include "PwmIn.h" #include "MathHelpers.h" #include "Transforms.h" #include "DQMapper.h" #include "ThrottleMapper.h" #include "BREMSStructs.h" #include "BREMSConfig.h" #include "config_motor.h" #include "config_loop.h" #include "config_pins.h" #include "config_inverter.h" #include "config_driving.h" #include "main.h" IOStruct io; ReadDataStruct read; FOCStruct foc; ControlStruct control; DQMapper *dq; ThrottleMapper *th; int loop_counter = 0; bool control_enabled = false; 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_filter(read.w, w_raw, W_FILTER_STRENGTH); } void commutate() { /*safety checks*/ if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) { /*do this even in disabled state, to keep integral down*/ go_disabled(); } else if (!control_enabled) { go_enabled(); } /*update velocity, references*/ update_velocity(); if (loop_counter % SLOW_LOOP_COUNTER == 0) { loop_counter = 0; slow_loop(); } loop_counter++; /*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_integral *= 1.0f - INTEGRAL_DECAY; control.q_integral *= 1.0f - INTEGRAL_DECAY; control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH); control.q_filtered = update_filter(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 * read.w * foc.q / BUS_VOLTAGE / 2.0f; foc.vq = KP * q_err + control.q_integral;// + Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f; 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;//don't think about it 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 slow_loop() { float torque_percent = th->map(io.throttle_in->get_throttle(), read.w); dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref); } void go_enabled() { control_enabled = true; io.en->write(1); } void go_disabled() { control.d_integral = 0.0f; control.q_integral = 0.0f; control_enabled = false; io.en->write(0); } bool is_driving() { return io.throttle_in->get_throttle() > 0.01f || fabsf(read.w) > W_SAFE; } float update_filter(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++); read.adval1 = ADC1->DR; read.adval2 = ADC2->DR; commutate(); } TIM1->SR = 0x00; } int main() { dq = new LutMapper();// LinearNoFWMapper(KT, TORQUE_MAX, FLUX_LINKAGE); th = new NullThrottleMapper(); BREMSInit(&io, &read, &foc, &control, false); for (;;) { } }