Bayley Wang
/
foc-ed_in_the_bot_compact
robot
main.cpp
- Committer:
- bwang
- Date:
- 2018-02-10
- Revision:
- 195:b61a734eb50c
- Parent:
- 194:05e119bc5a78
- Child:
- 196:7172e6e28867
File content as of revision 195:b61a734eb50c:
#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 "prefs.h" #include "layout.h" #include "derived.h" #include "main.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 (!control.enabled && checks_passed()) { 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 (!checks_passed()) { /*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); /*log data*/ if (_ENABLE_LOGGING) log(); } 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! 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(); } }