Bayley Wang
/
foc-ed_in_the_bot_compact
robot
main.cpp
- Committer:
- bwang
- Date:
- 2017-07-01
- Revision:
- 160:6948bb7bcabd
- Parent:
- 158:882f9c208378
- Child:
- 161:19eac809c727
File content as of revision 160:6948bb7bcabd:
#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 "Calibration.h" #include "Filter.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 "config_logging.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 read.w = control.velocity_filter->update(w_raw); } void commutate() { /*safety checks, do we do anything this cycle?*/ if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) { 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_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_D; control.q_integral += q_err * KI_Q; constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, INTEGRAL_MAX); foc.vd_decouple = -Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f; foc.vq_decouple = Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f; constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f); foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple; foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple; constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + OVERMODULATION_FACTOR); if (!control_enabled) { foc.vd = 0.0f; foc.vq = 0.0f; } float pv = foc.p + read.w / V_PHASE_SWIZZLE; float sin_pv = sinf(pv); float cos_pv = cosf(pv); /*inverse transforms*/ invpark(foc.vd, foc.vq, sin_pv, cos_pv, &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; /*safety checks, reset integral*/ if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) { /*do this even in disabled state, to keep integral down*/ go_disabled(); } /*output to timers*/ set_dtc(io.a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX); set_dtc(io.b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX); set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX); } void slow_loop() { float x = io.throttle_in->get_throttle(); x = control.throttle_filter->update(x); control.torque_percent = th->map(x, read.w); dq->map(control.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 control.torque_percent > 0.01f || fabsf(read.w) > W_SAFE; } float update_filter(float old, float x, float str) { return str * old + (1.0f - str) * x; } void log() { //io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), // (int) (255 * foc.vd), (int) (255 * foc.vq)); io.pc->printf("%c,%c\n", (int) (control.d_filtered / 2.0f), (int) (control.q_filtered / 2.0f)); //io.pc->printf("%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_filtered, (int) control.q_filtered, (int) (255 * foc.vd), (int) (255 * foc.vq)); wait(1.0f / LOG_FREQUENCY); } extern "C" void TIM1_UP_TIM10_IRQHandler(void) { int start_state = io.throttle_in->state(); 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 end_state = io.throttle_in->state(); if (start_state != end_state) io.throttle_in->block(); } int main() { dq = new AutoMapper(0.5f * PI, PI, 25, 140);//LutMapper(); th = new AutoThrottleMapper(50.0f, 3.0f);//NullThrottleMapper(); BREMSInit(&io, &read, &foc, &control, false); for (;;) { if (ENABLE_LOGGING) { log(); } } }