Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@252:38644631ed97, 2018-11-13 (annotated)

Committer:
bwang
Date:
Tue Nov 13 17:46:23 2018 +0000
Revision:
252:38644631ed97
Parent:
248:c4539ee69052 
11/13/2018 12:45 - hitting <return> on empty line prints "\r>", so that hitting enter after intially connecting to the controller generates a prompt

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