Bayley Wang
/
foc-ed_in_the_bot_compact
robot
main.cpp
- Committer:
- bwang
- Date:
- 2018-11-10
- Revision:
- 233:2ca26ab47b4f
- Parent:
- 232:47f6cf4f9126
- Child:
- 234:ad68d7f8eff1
File content as of revision 233:2ca26ab47b4f:
#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 "LedBlinker.h" #include "BREMSStructs.h" #include "BREMSConfig.h" #include "prefs.h" #include "hardware.h" #include "derived.h" #include "main.h" #include "errors.h" IOStruct io; ReadDataStruct read; FOCStruct foc; ControlStruct control; DQMapper *dq; ThrottleMapper *th; int loop_counter = 0; void commutate() { /*safety checks, do we do anything this cycle?*/ if (checks_passed()) { go_enabled(); } /*update velocity, references*/ update_velocity(); if (loop_counter % SLOW_LOOP_COUNTER == 0) { loop_counter = 0; slow_loop(); } loop_counter++; io.blink->update(); /*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.ad1_supp_offset) / I_SCALE; foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ad2_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); 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); /*set outputs*/ 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 (!checks_passed() || !mode_enables_output()) { go_disabled(); } /*log data*/ if (_ENABLE_LOGGING && mode_enables_logging()) log(); /*disable outputs if necessary*/ if (!control.enabled) { va = 0.0f; vb = 0.0f; vc = 0.0f; } //output to timers (some of the calibration modes may want to override this) if (!mode_overrides_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() { switch (BREMS_src) { case CMD_SRC_RC: control.user_cmd = control.throttle_filter->update(io.throttle_in->get_throttle()); break; case CMD_SRC_ANALOG: //handle analog throttle here break; case CMD_SRC_TERMINAL: case CMD_SRC_SERIAL: case CMD_SRC_CAN: case CMD_SRC_INTERNAL: //these sources are updated by callbacks break; default: //this should never happen! control.user_cmd = 0.0f; break; } control.torque_percent = th->map(control.user_cmd, read.w); dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref); } void log() { float packet[8]; packet[0] = read.w / 8.0f; packet[1] = control.d_ref / 2.0f + 128.0f; packet[2] = control.d_filtered / 2.0f + 128.0f; packet[3] = control.q_ref / 2.0f + 128.0f; packet[4] = control.q_filtered / 2.0f + 128.0f; packet[5] = 255.0f * control.torque_percent; packet[6] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f; packet[7] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f; io.logger->log(packet); } int main() { dq = new InterpolatingLutMapper(); th = new LimitingThrottleMapper(1500.0f); BREMSInit(&io, &read, &foc, &control, false); io.pc->attach(rxCallback); for (;;) { io.logger->flush(); } }