Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@233:2ca26ab47b4f, 2018-11-10 (annotated)

Committer:
bwang
Date:
Sat Nov 10 09:15:26 2018 +0000
Revision:
233:2ca26ab47b4f
Parent:
232:47f6cf4f9126
Child:
234:ad68d7f8eff1
11/10/2018 04:14 - 'some modes override FOC outputs' removed from commutate(), as it is no longer needed (ZERO, CHR have their own routines, CFG disables output anyway)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 0:bac9c3a3a6ca 1 #include "mbed.h"
bwang 0:bac9c3a3a6ca 2 #include "math.h"
bwang 19:a6cf15f89f3d 3
bwang 0:bac9c3a3a6ca 4 #include "PositionSensor.h"
bwang 0:bac9c3a3a6ca 5 #include "FastPWM.h"
bwang 18:3863ca45cf26 6 #include "PwmIn.h"
bwang 19:a6cf15f89f3d 7 #include "MathHelpers.h"
bwang 42:030e0ec4eac5 8 #include "Transforms.h"
bwang 42:030e0ec4eac5 9 #include "DQMapper.h"
bwang 42:030e0ec4eac5 10 #include "ThrottleMapper.h"
bwang 118:2b6dab10b69d 11 #include "Calibration.h"
bwang 150:08c13bfc7417 12 #include "Filter.h"
bwang 223:b986e7cee521 13 #include "LedBlinker.h"
bwang 42:030e0ec4eac5 14
bwang 42:030e0ec4eac5 15 #include "BREMSStructs.h"
bwang 42:030e0ec4eac5 16 #include "BREMSConfig.h"
bwang 30:c25c5bf0d951 17
bwang 194:05e119bc5a78 18 #include "prefs.h"
bwang 196:7172e6e28867 19 #include "hardware.h"
bwang 185:5c102874b490 20 #include "derived.h"
bwang 42:030e0ec4eac5 21 #include "main.h"
bwang 0:bac9c3a3a6ca 22
bwang 232:47f6cf4f9126 23 #include "errors.h"
bwang 232:47f6cf4f9126 24
bwang 42:030e0ec4eac5 25 IOStruct io;
bwang 42:030e0ec4eac5 26 ReadDataStruct read;
bwang 42:030e0ec4eac5 27 FOCStruct foc;
bwang 42:030e0ec4eac5 28 ControlStruct control;
bwang 2:eabe8feaaabb 29
bwang 42:030e0ec4eac5 30 DQMapper *dq;
bwang 42:030e0ec4eac5 31 ThrottleMapper *th;
bwang 2:eabe8feaaabb 32
bwang 44:3fd6a43b91f0 33 int loop_counter = 0;
bwang 2:eabe8feaaabb 34
bwang 118:2b6dab10b69d 35 void commutate() {
bwang 56:c681001dfa46 36 /*safety checks, do we do anything this cycle?*/
bwang 198:7ee146427a0d 37 if (checks_passed()) {
bwang 52:fd3d8df99287 38 go_enabled();
bwang 52:fd3d8df99287 39 }
bwang 42:030e0ec4eac5 40
bwang 42:030e0ec4eac5 41 /*update velocity, references*/
bwang 42:030e0ec4eac5 42 update_velocity();
bwang 44:3fd6a43b91f0 43 if (loop_counter % SLOW_LOOP_COUNTER == 0) {
bwang 44:3fd6a43b91f0 44 loop_counter = 0;
bwang 44:3fd6a43b91f0 45 slow_loop();
bwang 44:3fd6a43b91f0 46 }
bwang 44:3fd6a43b91f0 47 loop_counter++;
bwang 223:b986e7cee521 48 io.blink->update();
bwang 223:b986e7cee521 49
bwang 42:030e0ec4eac5 50 /*update position, sin, cos*/
bwang 193:3abadeecf908 51 foc.p = io.pos->GetElecPosition() - _POS_OFFSET;
bwang 42:030e0ec4eac5 52 float sin_p = sinf(foc.p);
bwang 42:030e0ec4eac5 53 float cos_p = cosf(foc.p);
bwang 42:030e0ec4eac5 54
bwang 49:da8604278d76 55 /*scale and offset currents (adval1, 2 are updated in ISR)*/
bwang 231:753ec371b153 56 foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ad1_supp_offset) / I_SCALE;
bwang 231:753ec371b153 57 foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ad2_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 58
bwang 42:030e0ec4eac5 59 /*compute d, q*/
bwang 42:030e0ec4eac5 60 clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
bwang 42:030e0ec4eac5 61 park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
bwang 42:030e0ec4eac5 62
bwang 42:030e0ec4eac5 63 /*PI controller*/
bwang 193:3abadeecf908 64 control.d_filtered = update_filter(control.d_filtered, foc.d, _DQ_FILTER_STRENGTH);
bwang 193:3abadeecf908 65 control.q_filtered = update_filter(control.q_filtered, foc.q, _DQ_FILTER_STRENGTH);
bwang 42:030e0ec4eac5 66
bwang 42:030e0ec4eac5 67 float d_err = control.d_ref - control.d_filtered;
bwang 42:030e0ec4eac5 68 float q_err = control.q_ref - control.q_filtered;
bwang 42:030e0ec4eac5 69
bwang 58:7316c5a4c417 70 control.d_integral += d_err * KI_D;
bwang 58:7316c5a4c417 71 control.q_integral += q_err * KI_Q;
bwang 42:030e0ec4eac5 72
bwang 193:3abadeecf908 73 constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, _INTEGRAL_MAX);
bwang 131:031df63c7dbc 74
bwang 193:3abadeecf908 75 foc.vd_decouple = -_Lq * _POLE_PAIRS * read.w * foc.q / _BUS_VOLTAGE / 2.0f;
bwang 193:3abadeecf908 76 foc.vq_decouple = _Ld * _POLE_PAIRS * read.w * foc.d / _BUS_VOLTAGE / 2.0f;
bwang 42:030e0ec4eac5 77
bwang 158:882f9c208378 78 constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f);
bwang 131:031df63c7dbc 79
bwang 131:031df63c7dbc 80 foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple;
bwang 131:031df63c7dbc 81 foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple;
bwang 42:030e0ec4eac5 82
bwang 193:3abadeecf908 83 constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + _OVERMODULATION_FACTOR);
bwang 42:030e0ec4eac5 84
bwang 193:3abadeecf908 85 float pv = foc.p + read.w / _V_PHASE_SWIZZLE;
bwang 124:e70ca81676fc 86 float sin_pv = sinf(pv);
bwang 124:e70ca81676fc 87 float cos_pv = cosf(pv);
bwang 124:e70ca81676fc 88
bwang 42:030e0ec4eac5 89 /*inverse transforms*/
bwang 124:e70ca81676fc 90 invpark(foc.vd, foc.vq, sin_pv, cos_pv, &foc.valpha, &foc.vbeta);
bwang 202:1baff0df86d1 91
bwang 202:1baff0df86d1 92 /*set outputs*/
bwang 42:030e0ec4eac5 93 float va, vb, vc, voff;
bwang 204:74714d52a936 94
bwang 204:74714d52a936 95 invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang 204:74714d52a936 96 vc = -va - vb;
bwang 204:74714d52a936 97
bwang 204:74714d52a936 98 /*SVPWM*/
bwang 204:74714d52a936 99 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 204:74714d52a936 100 va = va - voff;
bwang 204:74714d52a936 101 vb = vb - voff;
bwang 204:74714d52a936 102 vc = vc - voff;
bwang 205:5cfe6d7e08a3 103
bwang 56:c681001dfa46 104 /*safety checks, reset integral*/
bwang 204:74714d52a936 105 if (!checks_passed() || !mode_enables_output()) {
bwang 198:7ee146427a0d 106 go_disabled();
bwang 198:7ee146427a0d 107 }
bwang 198:7ee146427a0d 108
bwang 180:a783a972a867 109 /*log data*/
bwang 204:74714d52a936 110 if (_ENABLE_LOGGING && mode_enables_logging()) log();
bwang 201:5d1a3aa50fe0 111
bwang 202:1baff0df86d1 112 /*disable outputs if necessary*/
bwang 201:5d1a3aa50fe0 113 if (!control.enabled) {
bwang 201:5d1a3aa50fe0 114 va = 0.0f; vb = 0.0f; vc = 0.0f;
bwang 201:5d1a3aa50fe0 115 }
bwang 201:5d1a3aa50fe0 116
bwang 232:47f6cf4f9126 117 //output to timers (some of the calibration modes may want to override this)
bwang 232:47f6cf4f9126 118 if (!mode_overrides_timers()) {
bwang 232:47f6cf4f9126 119 set_dtc(io.a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 120 set_dtc(io.b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 121 set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 122 }
bwang 42:030e0ec4eac5 123 }
bwang 42:030e0ec4eac5 124
bwang 44:3fd6a43b91f0 125 void slow_loop() {
bwang 194:05e119bc5a78 126 switch (BREMS_src) {
bwang 194:05e119bc5a78 127 case CMD_SRC_RC:
bwang 194:05e119bc5a78 128 control.user_cmd = control.throttle_filter->update(io.throttle_in->get_throttle());
bwang 194:05e119bc5a78 129 break;
bwang 194:05e119bc5a78 130 case CMD_SRC_ANALOG:
bwang 194:05e119bc5a78 131 //handle analog throttle here
bwang 194:05e119bc5a78 132 break;
bwang 194:05e119bc5a78 133 case CMD_SRC_TERMINAL:
bwang 194:05e119bc5a78 134 case CMD_SRC_SERIAL:
bwang 194:05e119bc5a78 135 case CMD_SRC_CAN:
bwang 194:05e119bc5a78 136 case CMD_SRC_INTERNAL:
bwang 194:05e119bc5a78 137 //these sources are updated by callbacks
bwang 194:05e119bc5a78 138 break;
bwang 194:05e119bc5a78 139 default:
bwang 194:05e119bc5a78 140 //this should never happen!
bwang 199:c160a2c03781 141 control.user_cmd = 0.0f;
bwang 194:05e119bc5a78 142 break;
bwang 194:05e119bc5a78 143 }
bwang 194:05e119bc5a78 144
bwang 194:05e119bc5a78 145 control.torque_percent = th->map(control.user_cmd, read.w);
bwang 70:5e39beeb4a21 146 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang 44:3fd6a43b91f0 147 }
bwang 44:3fd6a43b91f0 148
bwang 72:5f1da97d62e1 149 void log() {
bwang 180:a783a972a867 150 float packet[8];
bwang 180:a783a972a867 151 packet[0] = read.w / 8.0f;
bwang 180:a783a972a867 152 packet[1] = control.d_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 153 packet[2] = control.d_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 154 packet[3] = control.q_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 155 packet[4] = control.q_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 156 packet[5] = 255.0f * control.torque_percent;
bwang 193:3abadeecf908 157 packet[6] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f;
bwang 193:3abadeecf908 158 packet[7] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f;
bwang 180:a783a972a867 159 io.logger->log(packet);
bwang 70:5e39beeb4a21 160 }
bwang 1:7b61790f6be9 161
bwang 166:4637785ba01e 162 int main() {
bwang 213:2218bab57355 163 dq = new InterpolatingLutMapper();
bwang 174:3872516b0d04 164 th = new LimitingThrottleMapper(1500.0f);
bwang 180:a783a972a867 165
bwang 47:1c9868e226d0 166 BREMSInit(&io, &read, &foc, &control, false);
bwang 181:d3510c8beab6 167 io.pc->attach(rxCallback);
bwang 223:b986e7cee521 168
bwang 0:bac9c3a3a6ca 169 for (;;) {
bwang 180:a783a972a867 170 io.logger->flush();
bwang 0:bac9c3a3a6ca 171 }
bwang 42:030e0ec4eac5 172 }