Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@229:90c6892f4d3b, 2018-11-10 (annotated)

Committer:
bwang
Date:
Sat Nov 10 06:27:07 2018 +0000
Revision:
229:90c6892f4d3b
Parent:
228:8a14f1da2121
Child:
230:7ac00598d366
11/10/2018 01:26 - renamed ia_supp_offset, ib_supp_offset to ad1, ad2_supp_offset to reflect the fact that they correspond to ADC channel offsets and not currents (which can be renamed in elsewhere)

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