Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@243:96b03b3bf3ef, 2018-11-12 (annotated)

Committer:
bwang
Date:
Mon Nov 12 11:16:43 2018 +0000
Revision:
243:96b03b3bf3ef
Parent:
242:ac30f04fd6f7
Child:
248:c4539ee69052
11/12/2018 06:16 - added W_LOOP_MAX_TQ parameter to limit acceleration in speed mode (actually, to make bench supplies not freak out when the motor ramps)

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 240:2aaffa217627 41 /*update velocity, user command*/
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 240:2aaffa217627 49
bwang 240:2aaffa217627 50 /*generate torque command from user command*/
bwang 240:2aaffa217627 51 float w_ref, w_err;
bwang 240:2aaffa217627 52 switch (BREMS_op) {
bwang 240:2aaffa217627 53 case OP_TORQUE:
bwang 240:2aaffa217627 54 control.w_integral = 0.0f;
bwang 240:2aaffa217627 55 control.torque_percent = control.user_cmd;
bwang 240:2aaffa217627 56 break;
bwang 240:2aaffa217627 57 case OP_DRIVING:
bwang 240:2aaffa217627 58 control.w_integral = 0.0f;
bwang 240:2aaffa217627 59 control.torque_percent = th->map(control.user_cmd, read.w);
bwang 240:2aaffa217627 60 break;
bwang 240:2aaffa217627 61 case OP_SPEED:
bwang 240:2aaffa217627 62 w_ref = control.user_cmd * _W_SETPOINT_MAX;
bwang 240:2aaffa217627 63 w_err = w_ref - read.w;
bwang 240:2aaffa217627 64 control.w_integral += w_err * KI_W;
bwang 243:96b03b3bf3ef 65 control.w_integral = constrain(control.w_integral, -_W_LOOP_MAX_TQ, _W_LOOP_MAX_TQ);
bwang 240:2aaffa217627 66 control.torque_percent = KP_W * w_err + control.w_integral;
bwang 243:96b03b3bf3ef 67 control.torque_percent = constrain(control.torque_percent, -_W_LOOP_MAX_TQ, _W_LOOP_MAX_TQ);
bwang 240:2aaffa217627 68 break;
bwang 240:2aaffa217627 69 case OP_POSITION:
bwang 240:2aaffa217627 70 break;
bwang 240:2aaffa217627 71 default:
bwang 240:2aaffa217627 72 //this should never happen!
bwang 240:2aaffa217627 73 control.torque_percent = 0.0f;
bwang 240:2aaffa217627 74 break;
bwang 240:2aaffa217627 75 }
bwang 240:2aaffa217627 76
bwang 240:2aaffa217627 77 /*update references*/
bwang 240:2aaffa217627 78 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang 223:b986e7cee521 79
bwang 42:030e0ec4eac5 80 /*update position, sin, cos*/
bwang 193:3abadeecf908 81 foc.p = io.pos->GetElecPosition() - _POS_OFFSET;
bwang 42:030e0ec4eac5 82 float sin_p = sinf(foc.p);
bwang 42:030e0ec4eac5 83 float cos_p = cosf(foc.p);
bwang 42:030e0ec4eac5 84
bwang 49:da8604278d76 85 /*scale and offset currents (adval1, 2 are updated in ISR)*/
bwang 231:753ec371b153 86 foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ad1_supp_offset) / I_SCALE;
bwang 231:753ec371b153 87 foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ad2_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 88
bwang 42:030e0ec4eac5 89 /*compute d, q*/
bwang 42:030e0ec4eac5 90 clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
bwang 42:030e0ec4eac5 91 park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
bwang 42:030e0ec4eac5 92
bwang 42:030e0ec4eac5 93 /*PI controller*/
bwang 193:3abadeecf908 94 control.d_filtered = update_filter(control.d_filtered, foc.d, _DQ_FILTER_STRENGTH);
bwang 193:3abadeecf908 95 control.q_filtered = update_filter(control.q_filtered, foc.q, _DQ_FILTER_STRENGTH);
bwang 42:030e0ec4eac5 96
bwang 42:030e0ec4eac5 97 float d_err = control.d_ref - control.d_filtered;
bwang 42:030e0ec4eac5 98 float q_err = control.q_ref - control.q_filtered;
bwang 42:030e0ec4eac5 99
bwang 58:7316c5a4c417 100 control.d_integral += d_err * KI_D;
bwang 58:7316c5a4c417 101 control.q_integral += q_err * KI_Q;
bwang 42:030e0ec4eac5 102
bwang 193:3abadeecf908 103 constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, _INTEGRAL_MAX);
bwang 131:031df63c7dbc 104
bwang 193:3abadeecf908 105 foc.vd_decouple = -_Lq * _POLE_PAIRS * read.w * foc.q / _BUS_VOLTAGE / 2.0f;
bwang 193:3abadeecf908 106 foc.vq_decouple = _Ld * _POLE_PAIRS * read.w * foc.d / _BUS_VOLTAGE / 2.0f;
bwang 42:030e0ec4eac5 107
bwang 158:882f9c208378 108 constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f);
bwang 131:031df63c7dbc 109
bwang 131:031df63c7dbc 110 foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple;
bwang 131:031df63c7dbc 111 foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple;
bwang 42:030e0ec4eac5 112
bwang 193:3abadeecf908 113 constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + _OVERMODULATION_FACTOR);
bwang 42:030e0ec4eac5 114
bwang 193:3abadeecf908 115 float pv = foc.p + read.w / _V_PHASE_SWIZZLE;
bwang 124:e70ca81676fc 116 float sin_pv = sinf(pv);
bwang 124:e70ca81676fc 117 float cos_pv = cosf(pv);
bwang 124:e70ca81676fc 118
bwang 42:030e0ec4eac5 119 /*inverse transforms*/
bwang 124:e70ca81676fc 120 invpark(foc.vd, foc.vq, sin_pv, cos_pv, &foc.valpha, &foc.vbeta);
bwang 202:1baff0df86d1 121
bwang 202:1baff0df86d1 122 /*set outputs*/
bwang 42:030e0ec4eac5 123 float va, vb, vc, voff;
bwang 204:74714d52a936 124
bwang 204:74714d52a936 125 invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang 204:74714d52a936 126 vc = -va - vb;
bwang 204:74714d52a936 127
bwang 204:74714d52a936 128 /*SVPWM*/
bwang 204:74714d52a936 129 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 204:74714d52a936 130 va = va - voff;
bwang 204:74714d52a936 131 vb = vb - voff;
bwang 204:74714d52a936 132 vc = vc - voff;
bwang 205:5cfe6d7e08a3 133
bwang 56:c681001dfa46 134 /*safety checks, reset integral*/
bwang 204:74714d52a936 135 if (!checks_passed() || !mode_enables_output()) {
bwang 198:7ee146427a0d 136 go_disabled();
bwang 198:7ee146427a0d 137 }
bwang 198:7ee146427a0d 138
bwang 180:a783a972a867 139 /*log data*/
bwang 204:74714d52a936 140 if (_ENABLE_LOGGING && mode_enables_logging()) log();
bwang 201:5d1a3aa50fe0 141
bwang 202:1baff0df86d1 142 /*disable outputs if necessary*/
bwang 201:5d1a3aa50fe0 143 if (!control.enabled) {
bwang 201:5d1a3aa50fe0 144 va = 0.0f; vb = 0.0f; vc = 0.0f;
bwang 201:5d1a3aa50fe0 145 }
bwang 201:5d1a3aa50fe0 146
bwang 232:47f6cf4f9126 147 //output to timers (some of the calibration modes may want to override this)
bwang 234:ad68d7f8eff1 148 //CFG does not override timers - the PWM outputs need to be set to 0 somewhere
bwang 232:47f6cf4f9126 149 if (!mode_overrides_timers()) {
bwang 232:47f6cf4f9126 150 set_dtc(io.a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 151 set_dtc(io.b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 152 set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 153 }
bwang 42:030e0ec4eac5 154 }
bwang 42:030e0ec4eac5 155
bwang 44:3fd6a43b91f0 156 void slow_loop() {
bwang 194:05e119bc5a78 157 switch (BREMS_src) {
bwang 194:05e119bc5a78 158 case CMD_SRC_RC:
bwang 194:05e119bc5a78 159 control.user_cmd = control.throttle_filter->update(io.throttle_in->get_throttle());
bwang 194:05e119bc5a78 160 break;
bwang 194:05e119bc5a78 161 case CMD_SRC_ANALOG:
bwang 194:05e119bc5a78 162 //handle analog throttle here
bwang 194:05e119bc5a78 163 break;
bwang 194:05e119bc5a78 164 case CMD_SRC_TERMINAL:
bwang 194:05e119bc5a78 165 case CMD_SRC_SERIAL:
bwang 194:05e119bc5a78 166 case CMD_SRC_CAN:
bwang 194:05e119bc5a78 167 case CMD_SRC_INTERNAL:
bwang 194:05e119bc5a78 168 //these sources are updated by callbacks
bwang 194:05e119bc5a78 169 break;
bwang 194:05e119bc5a78 170 default:
bwang 194:05e119bc5a78 171 //this should never happen!
bwang 199:c160a2c03781 172 control.user_cmd = 0.0f;
bwang 194:05e119bc5a78 173 break;
bwang 194:05e119bc5a78 174 }
bwang 44:3fd6a43b91f0 175 }
bwang 44:3fd6a43b91f0 176
bwang 72:5f1da97d62e1 177 void log() {
bwang 180:a783a972a867 178 float packet[8];
bwang 180:a783a972a867 179 packet[0] = read.w / 8.0f;
bwang 180:a783a972a867 180 packet[1] = control.d_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 181 packet[2] = control.d_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 182 packet[3] = control.q_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 183 packet[4] = control.q_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 184 packet[5] = 255.0f * control.torque_percent;
bwang 193:3abadeecf908 185 packet[6] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f;
bwang 193:3abadeecf908 186 packet[7] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f;
bwang 180:a783a972a867 187 io.logger->log(packet);
bwang 70:5e39beeb4a21 188 }
bwang 1:7b61790f6be9 189
bwang 166:4637785ba01e 190 int main() {
bwang 213:2218bab57355 191 dq = new InterpolatingLutMapper();
bwang 174:3872516b0d04 192 th = new LimitingThrottleMapper(1500.0f);
bwang 180:a783a972a867 193
bwang 47:1c9868e226d0 194 BREMSInit(&io, &read, &foc, &control, false);
bwang 181:d3510c8beab6 195 io.pc->attach(rxCallback);
bwang 223:b986e7cee521 196
bwang 0:bac9c3a3a6ca 197 for (;;) {
bwang 180:a783a972a867 198 io.logger->flush();
bwang 0:bac9c3a3a6ca 199 }
bwang 42:030e0ec4eac5 200 }