Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@234:ad68d7f8eff1, 2018-11-10 (annotated)

Committer:
bwang
Date:
Sat Nov 10 09:21:51 2018 +0000
Revision:
234:ad68d7f8eff1
Parent:
233:2ca26ab47b4f
Child:
240:2aaffa217627
11/10/2018 04:21 - cmd_setp() sets setpoint to zero if there are errors present (to avoid sudden start of motor if errors are cleared)

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 234:ad68d7f8eff1 118 //CFG does not override timers - the PWM outputs need to be set to 0 somewhere
bwang 232:47f6cf4f9126 119 if (!mode_overrides_timers()) {
bwang 232:47f6cf4f9126 120 set_dtc(io.a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 121 set_dtc(io.b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 122 set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
bwang 232:47f6cf4f9126 123 }
bwang 42:030e0ec4eac5 124 }
bwang 42:030e0ec4eac5 125
bwang 44:3fd6a43b91f0 126 void slow_loop() {
bwang 194:05e119bc5a78 127 switch (BREMS_src) {
bwang 194:05e119bc5a78 128 case CMD_SRC_RC:
bwang 194:05e119bc5a78 129 control.user_cmd = control.throttle_filter->update(io.throttle_in->get_throttle());
bwang 194:05e119bc5a78 130 break;
bwang 194:05e119bc5a78 131 case CMD_SRC_ANALOG:
bwang 194:05e119bc5a78 132 //handle analog throttle here
bwang 194:05e119bc5a78 133 break;
bwang 194:05e119bc5a78 134 case CMD_SRC_TERMINAL:
bwang 194:05e119bc5a78 135 case CMD_SRC_SERIAL:
bwang 194:05e119bc5a78 136 case CMD_SRC_CAN:
bwang 194:05e119bc5a78 137 case CMD_SRC_INTERNAL:
bwang 194:05e119bc5a78 138 //these sources are updated by callbacks
bwang 194:05e119bc5a78 139 break;
bwang 194:05e119bc5a78 140 default:
bwang 194:05e119bc5a78 141 //this should never happen!
bwang 199:c160a2c03781 142 control.user_cmd = 0.0f;
bwang 194:05e119bc5a78 143 break;
bwang 194:05e119bc5a78 144 }
bwang 194:05e119bc5a78 145
bwang 194:05e119bc5a78 146 control.torque_percent = th->map(control.user_cmd, read.w);
bwang 70:5e39beeb4a21 147 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang 44:3fd6a43b91f0 148 }
bwang 44:3fd6a43b91f0 149
bwang 72:5f1da97d62e1 150 void log() {
bwang 180:a783a972a867 151 float packet[8];
bwang 180:a783a972a867 152 packet[0] = read.w / 8.0f;
bwang 180:a783a972a867 153 packet[1] = control.d_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 154 packet[2] = control.d_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 155 packet[3] = control.q_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 156 packet[4] = control.q_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 157 packet[5] = 255.0f * control.torque_percent;
bwang 193:3abadeecf908 158 packet[6] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f;
bwang 193:3abadeecf908 159 packet[7] = 255.0f / (1.0f + _OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f;
bwang 180:a783a972a867 160 io.logger->log(packet);
bwang 70:5e39beeb4a21 161 }
bwang 1:7b61790f6be9 162
bwang 166:4637785ba01e 163 int main() {
bwang 213:2218bab57355 164 dq = new InterpolatingLutMapper();
bwang 174:3872516b0d04 165 th = new LimitingThrottleMapper(1500.0f);
bwang 180:a783a972a867 166
bwang 47:1c9868e226d0 167 BREMSInit(&io, &read, &foc, &control, false);
bwang 181:d3510c8beab6 168 io.pc->attach(rxCallback);
bwang 223:b986e7cee521 169
bwang 0:bac9c3a3a6ca 170 for (;;) {
bwang 180:a783a972a867 171 io.logger->flush();
bwang 0:bac9c3a3a6ca 172 }
bwang 42:030e0ec4eac5 173 }