Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@201:5d1a3aa50fe0, 2018-02-12 (annotated)

Committer:
bwang
Date:
Mon Feb 12 00:02:50 2018 +0000
Revision:
201:5d1a3aa50fe0
Parent:
199:c160a2c03781
Child:
202:1baff0df86d1
temp commit

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 42:030e0ec4eac5 13
bwang 42:030e0ec4eac5 14 #include "BREMSStructs.h"
bwang 42:030e0ec4eac5 15 #include "BREMSConfig.h"
bwang 30:c25c5bf0d951 16
bwang 194:05e119bc5a78 17 #include "prefs.h"
bwang 196:7172e6e28867 18 #include "hardware.h"
bwang 185:5c102874b490 19 #include "derived.h"
bwang 42:030e0ec4eac5 20 #include "main.h"
bwang 0:bac9c3a3a6ca 21
bwang 42:030e0ec4eac5 22 IOStruct io;
bwang 42:030e0ec4eac5 23 ReadDataStruct read;
bwang 42:030e0ec4eac5 24 FOCStruct foc;
bwang 42:030e0ec4eac5 25 ControlStruct control;
bwang 2:eabe8feaaabb 26
bwang 42:030e0ec4eac5 27 DQMapper *dq;
bwang 42:030e0ec4eac5 28 ThrottleMapper *th;
bwang 2:eabe8feaaabb 29
bwang 44:3fd6a43b91f0 30 int loop_counter = 0;
bwang 2:eabe8feaaabb 31
bwang 118:2b6dab10b69d 32 void commutate() {
bwang 56:c681001dfa46 33 /*safety checks, do we do anything this cycle?*/
bwang 198:7ee146427a0d 34 if (checks_passed()) {
bwang 52:fd3d8df99287 35 go_enabled();
bwang 52:fd3d8df99287 36 }
bwang 42:030e0ec4eac5 37
bwang 42:030e0ec4eac5 38 /*update velocity, references*/
bwang 42:030e0ec4eac5 39 update_velocity();
bwang 44:3fd6a43b91f0 40 if (loop_counter % SLOW_LOOP_COUNTER == 0) {
bwang 44:3fd6a43b91f0 41 loop_counter = 0;
bwang 44:3fd6a43b91f0 42 slow_loop();
bwang 44:3fd6a43b91f0 43 }
bwang 44:3fd6a43b91f0 44 loop_counter++;
bwang 42:030e0ec4eac5 45
bwang 42:030e0ec4eac5 46 /*update position, sin, cos*/
bwang 193:3abadeecf908 47 foc.p = io.pos->GetElecPosition() - _POS_OFFSET;
bwang 42:030e0ec4eac5 48 float sin_p = sinf(foc.p);
bwang 42:030e0ec4eac5 49 float cos_p = cosf(foc.p);
bwang 42:030e0ec4eac5 50
bwang 49:da8604278d76 51 /*scale and offset currents (adval1, 2 are updated in ISR)*/
bwang 42:030e0ec4eac5 52 foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ia_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 53 foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ib_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 54
bwang 42:030e0ec4eac5 55 /*compute d, q*/
bwang 42:030e0ec4eac5 56 clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
bwang 42:030e0ec4eac5 57 park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
bwang 42:030e0ec4eac5 58
bwang 42:030e0ec4eac5 59 /*PI controller*/
bwang 193:3abadeecf908 60 control.d_filtered = update_filter(control.d_filtered, foc.d, _DQ_FILTER_STRENGTH);
bwang 193:3abadeecf908 61 control.q_filtered = update_filter(control.q_filtered, foc.q, _DQ_FILTER_STRENGTH);
bwang 42:030e0ec4eac5 62
bwang 42:030e0ec4eac5 63 float d_err = control.d_ref - control.d_filtered;
bwang 42:030e0ec4eac5 64 float q_err = control.q_ref - control.q_filtered;
bwang 42:030e0ec4eac5 65
bwang 58:7316c5a4c417 66 control.d_integral += d_err * KI_D;
bwang 58:7316c5a4c417 67 control.q_integral += q_err * KI_Q;
bwang 42:030e0ec4eac5 68
bwang 193:3abadeecf908 69 constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, _INTEGRAL_MAX);
bwang 131:031df63c7dbc 70
bwang 193:3abadeecf908 71 foc.vd_decouple = -_Lq * _POLE_PAIRS * read.w * foc.q / _BUS_VOLTAGE / 2.0f;
bwang 193:3abadeecf908 72 foc.vq_decouple = _Ld * _POLE_PAIRS * read.w * foc.d / _BUS_VOLTAGE / 2.0f;
bwang 42:030e0ec4eac5 73
bwang 158:882f9c208378 74 constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f);
bwang 131:031df63c7dbc 75
bwang 131:031df63c7dbc 76 foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple;
bwang 131:031df63c7dbc 77 foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple;
bwang 42:030e0ec4eac5 78
bwang 193:3abadeecf908 79 constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + _OVERMODULATION_FACTOR);
bwang 42:030e0ec4eac5 80
bwang 201:5d1a3aa50fe0 81 /*
bwang 187:523cf8c962e4 82 if (!control.enabled) {
bwang 42:030e0ec4eac5 83 foc.vd = 0.0f;
bwang 42:030e0ec4eac5 84 foc.vq = 0.0f;
bwang 42:030e0ec4eac5 85 }
bwang 201:5d1a3aa50fe0 86 */
bwang 42:030e0ec4eac5 87
bwang 193:3abadeecf908 88 float pv = foc.p + read.w / _V_PHASE_SWIZZLE;
bwang 124:e70ca81676fc 89 float sin_pv = sinf(pv);
bwang 124:e70ca81676fc 90 float cos_pv = cosf(pv);
bwang 124:e70ca81676fc 91
bwang 42:030e0ec4eac5 92 /*inverse transforms*/
bwang 124:e70ca81676fc 93 invpark(foc.vd, foc.vq, sin_pv, cos_pv, &foc.valpha, &foc.vbeta);
bwang 42:030e0ec4eac5 94
bwang 42:030e0ec4eac5 95 float va, vb, vc, voff;
bwang 42:030e0ec4eac5 96
bwang 201:5d1a3aa50fe0 97 //invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang 201:5d1a3aa50fe0 98 //vc = -va - vb;
bwang 42:030e0ec4eac5 99
bwang 42:030e0ec4eac5 100 /*SVPWM*/
bwang 201:5d1a3aa50fe0 101 //voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 201:5d1a3aa50fe0 102 //va = va - voff;
bwang 201:5d1a3aa50fe0 103 //vb = vb - voff;
bwang 201:5d1a3aa50fe0 104 //vc = vc - voff;
bwang 201:5d1a3aa50fe0 105
bwang 201:5d1a3aa50fe0 106 /*set outputs*/
bwang 201:5d1a3aa50fe0 107 switch (BREMS_mode) {
bwang 201:5d1a3aa50fe0 108 case MODE_CFG:
bwang 201:5d1a3aa50fe0 109 va = 0.0f;
bwang 201:5d1a3aa50fe0 110 vb = 0.0f;
bwang 201:5d1a3aa50fe0 111 vc = 0.0f;
bwang 201:5d1a3aa50fe0 112 break;
bwang 201:5d1a3aa50fe0 113 case MODE_RUN:
bwang 201:5d1a3aa50fe0 114 invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang 201:5d1a3aa50fe0 115 vc = -va - vb;
bwang 201:5d1a3aa50fe0 116 /*SVPWM*/
bwang 201:5d1a3aa50fe0 117 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 201:5d1a3aa50fe0 118 va = va - voff;
bwang 201:5d1a3aa50fe0 119 vb = vb - voff;
bwang 201:5d1a3aa50fe0 120 vc = vc - voff;
bwang 201:5d1a3aa50fe0 121 break;
bwang 201:5d1a3aa50fe0 122 case MODE_ZERO:
bwang 201:5d1a3aa50fe0 123 va = 0.9f;
bwang 201:5d1a3aa50fe0 124 vb = -0.9f;
bwang 201:5d1a3aa50fe0 125 vc = -0.9f;
bwang 201:5d1a3aa50fe0 126 break;
bwang 201:5d1a3aa50fe0 127 case MODE_CHR:
bwang 201:5d1a3aa50fe0 128 //i have no idea lol;
bwang 201:5d1a3aa50fe0 129 break;
bwang 201:5d1a3aa50fe0 130 }
bwang 42:030e0ec4eac5 131
bwang 56:c681001dfa46 132 /*safety checks, reset integral*/
bwang 193:3abadeecf908 133 if (!checks_passed()) {
bwang 56:c681001dfa46 134 /*do this even in disabled state, to keep integral down*/
bwang 56:c681001dfa46 135 go_disabled();
bwang 56:c681001dfa46 136 }
bwang 56:c681001dfa46 137
bwang 198:7ee146427a0d 138 /*some modes disable output*/
bwang 198:7ee146427a0d 139 switch (BREMS_mode) {
bwang 198:7ee146427a0d 140 case MODE_CFG:
bwang 198:7ee146427a0d 141 go_disabled();
bwang 198:7ee146427a0d 142 break;
bwang 198:7ee146427a0d 143 case MODE_RUN:
bwang 198:7ee146427a0d 144 case MODE_ZERO:
bwang 198:7ee146427a0d 145 case MODE_CHR:
bwang 198:7ee146427a0d 146 break;
bwang 198:7ee146427a0d 147 default:
bwang 198:7ee146427a0d 148 go_disabled();
bwang 198:7ee146427a0d 149 break;
bwang 198:7ee146427a0d 150 }
bwang 198:7ee146427a0d 151
bwang 198:7ee146427a0d 152
bwang 180:a783a972a867 153 /*log data*/
bwang 198:7ee146427a0d 154 if (_ENABLE_LOGGING) {
bwang 198:7ee146427a0d 155 switch (BREMS_mode) {
bwang 198:7ee146427a0d 156 case MODE_RUN:
bwang 198:7ee146427a0d 157 log();
bwang 198:7ee146427a0d 158 break;
bwang 198:7ee146427a0d 159 case MODE_CFG:
bwang 198:7ee146427a0d 160 case MODE_ZERO:
bwang 198:7ee146427a0d 161 case MODE_CHR:
bwang 198:7ee146427a0d 162 default:
bwang 198:7ee146427a0d 163 break;
bwang 198:7ee146427a0d 164 }
bwang 198:7ee146427a0d 165 }
bwang 201:5d1a3aa50fe0 166
bwang 201:5d1a3aa50fe0 167 if (!control.enabled) {
bwang 201:5d1a3aa50fe0 168 va = 0.0f; vb = 0.0f; vc = 0.0f;
bwang 201:5d1a3aa50fe0 169 }
bwang 201:5d1a3aa50fe0 170
bwang 201:5d1a3aa50fe0 171 //output to timers
bwang 201:5d1a3aa50fe0 172 set_dtc(io.a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
bwang 201:5d1a3aa50fe0 173 set_dtc(io.b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
bwang 201:5d1a3aa50fe0 174 set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
bwang 42:030e0ec4eac5 175 }
bwang 42:030e0ec4eac5 176
bwang 44:3fd6a43b91f0 177 void slow_loop() {
bwang 194:05e119bc5a78 178 switch (BREMS_src) {
bwang 194:05e119bc5a78 179 case CMD_SRC_RC:
bwang 194:05e119bc5a78 180 control.user_cmd = control.throttle_filter->update(io.throttle_in->get_throttle());
bwang 194:05e119bc5a78 181 break;
bwang 194:05e119bc5a78 182 case CMD_SRC_ANALOG:
bwang 194:05e119bc5a78 183 //handle analog throttle here
bwang 194:05e119bc5a78 184 break;
bwang 194:05e119bc5a78 185 case CMD_SRC_TERMINAL:
bwang 194:05e119bc5a78 186 case CMD_SRC_SERIAL:
bwang 194:05e119bc5a78 187 case CMD_SRC_CAN:
bwang 194:05e119bc5a78 188 case CMD_SRC_INTERNAL:
bwang 194:05e119bc5a78 189 //these sources are updated by callbacks
bwang 194:05e119bc5a78 190 break;
bwang 194:05e119bc5a78 191 default:
bwang 194:05e119bc5a78 192 //this should never happen!
bwang 199:c160a2c03781 193 control.user_cmd = 0.0f;
bwang 194:05e119bc5a78 194 break;
bwang 194:05e119bc5a78 195 }
bwang 194:05e119bc5a78 196
bwang 194:05e119bc5a78 197 control.torque_percent = th->map(control.user_cmd, read.w);
bwang 70:5e39beeb4a21 198 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang 44:3fd6a43b91f0 199 }
bwang 44:3fd6a43b91f0 200
bwang 72:5f1da97d62e1 201 void log() {
bwang 180:a783a972a867 202 float packet[8];
bwang 180:a783a972a867 203 packet[0] = read.w / 8.0f;
bwang 180:a783a972a867 204 packet[1] = control.d_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 205 packet[2] = control.d_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 206 packet[3] = control.q_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 207 packet[4] = control.q_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 208 packet[5] = 255.0f * control.torque_percent;
bwang 193:3abadeecf908 209 packet[6] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f;
bwang 193:3abadeecf908 210 packet[7] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f;
bwang 180:a783a972a867 211 io.logger->log(packet);
bwang 70:5e39beeb4a21 212 }
bwang 1:7b61790f6be9 213
bwang 166:4637785ba01e 214 int main() {
bwang 172:3d7196b71afd 215 dq = new InterpolatingLutMapper();
bwang 174:3872516b0d04 216 th = new LimitingThrottleMapper(1500.0f);
bwang 180:a783a972a867 217
bwang 47:1c9868e226d0 218 BREMSInit(&io, &read, &foc, &control, false);
bwang 181:d3510c8beab6 219 io.pc->attach(rxCallback);
bwang 48:a1a09c83d42c 220
bwang 0:bac9c3a3a6ca 221 for (;;) {
bwang 180:a783a972a867 222 io.logger->flush();
bwang 0:bac9c3a3a6ca 223 }
bwang 42:030e0ec4eac5 224 }