Bayley Wang
/
foc-ed_in_the_bot_compact
robot
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?
User | Revision | Line number | New 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 | } |