Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@86:b059f637e9ac, 2017-03-12 (annotated)

Committer:
bwang
Date:
Sun Mar 12 08:54:19 2017 +0000
Revision:
86:b059f637e9ac
Parent:
84:dd32640942a4
Child:
87:eb5e918a6930
renamed some stuff

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 82:5e741c5ffd9f 11 #include "PreferenceWriter.h"
bwang 82:5e741c5ffd9f 12 #include "CommandProcessor.h"
bwang 86:b059f637e9ac 13
bwang 86:b059f637e9ac 14 #include "main.h"
bwang 86:b059f637e9ac 15 #include "prefs.h"
bwang 42:030e0ec4eac5 16
bwang 42:030e0ec4eac5 17 #include "BREMSStructs.h"
bwang 42:030e0ec4eac5 18 #include "BREMSConfig.h"
bwang 30:c25c5bf0d951 19
bwang 15:b583cd30b063 20 #include "config_motor.h"
bwang 15:b583cd30b063 21 #include "config_loop.h"
bwang 19:a6cf15f89f3d 22 #include "config_pins.h"
bwang 15:b583cd30b063 23 #include "config_inverter.h"
bwang 29:50e6e4e46580 24 #include "config_driving.h"
bwang 72:5f1da97d62e1 25 #include "config_logging.h"
bwang 0:bac9c3a3a6ca 26
bwang 42:030e0ec4eac5 27 IOStruct io;
bwang 42:030e0ec4eac5 28 ReadDataStruct read;
bwang 42:030e0ec4eac5 29 FOCStruct foc;
bwang 42:030e0ec4eac5 30 ControlStruct control;
bwang 2:eabe8feaaabb 31
bwang 42:030e0ec4eac5 32 DQMapper *dq;
bwang 42:030e0ec4eac5 33 ThrottleMapper *th;
bwang 2:eabe8feaaabb 34
bwang 44:3fd6a43b91f0 35 int loop_counter = 0;
dicarloj 13:41d102a53caf 36
bwang 42:030e0ec4eac5 37 void update_velocity() {
bwang 42:030e0ec4eac5 38 read.last_p_mech = read.p_mech;
bwang 42:030e0ec4eac5 39 read.p_mech = io.pos->GetMechPosition();
bwang 44:3fd6a43b91f0 40
bwang 42:030e0ec4eac5 41 float dp_mech = read.p_mech - read.last_p_mech;
bwang 42:030e0ec4eac5 42 if (dp_mech < -PI) dp_mech += 2 * PI;
bwang 42:030e0ec4eac5 43 if (dp_mech > PI) dp_mech -= 2 * PI;
bwang 44:3fd6a43b91f0 44
bwang 42:030e0ec4eac5 45 float w_raw = dp_mech * F_SW; //rad/s
bwang 42:030e0ec4eac5 46 if (w_raw > W_CRAZY) w_raw = read.w; //with this limiting scheme noise < 0
bwang 42:030e0ec4eac5 47 if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first
bwang 44:3fd6a43b91f0 48
bwang 44:3fd6a43b91f0 49 read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH);
bwang 42:030e0ec4eac5 50 }
bwang 2:eabe8feaaabb 51
bwang 75:591556ce033d 52 void commutate() {
bwang 56:c681001dfa46 53 /*safety checks, do we do anything this cycle?*/
bwang 84:dd32640942a4 54 if (!control.enabled && checks_passed()) {
bwang 52:fd3d8df99287 55 go_enabled();
bwang 52:fd3d8df99287 56 }
bwang 42:030e0ec4eac5 57
bwang 42:030e0ec4eac5 58 /*update velocity, references*/
bwang 42:030e0ec4eac5 59 update_velocity();
bwang 44:3fd6a43b91f0 60 if (loop_counter % SLOW_LOOP_COUNTER == 0) {
bwang 44:3fd6a43b91f0 61 loop_counter = 0;
bwang 44:3fd6a43b91f0 62 slow_loop();
bwang 44:3fd6a43b91f0 63 }
bwang 44:3fd6a43b91f0 64 loop_counter++;
bwang 42:030e0ec4eac5 65
bwang 42:030e0ec4eac5 66 /*update position, sin, cos*/
bwang 42:030e0ec4eac5 67 foc.p = io.pos->GetElecPosition() - POS_OFFSET;
bwang 42:030e0ec4eac5 68 float sin_p = sinf(foc.p);
bwang 42:030e0ec4eac5 69 float cos_p = cosf(foc.p);
bwang 42:030e0ec4eac5 70
bwang 49:da8604278d76 71 /*scale and offset currents (adval1, 2 are updated in ISR)*/
bwang 42:030e0ec4eac5 72 foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ia_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 73 foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ib_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 74
bwang 42:030e0ec4eac5 75 /*compute d, q*/
bwang 42:030e0ec4eac5 76 clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
bwang 42:030e0ec4eac5 77 park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
bwang 42:030e0ec4eac5 78
bwang 84:dd32640942a4 79 /*run additional controllers; the output of every controller is a torque percent*/
bwang 84:dd32640942a4 80 switch (BREMS_op) {
bwang 84:dd32640942a4 81 case OP_TORQUE:
bwang 84:dd32640942a4 82 control.torque_percent = control.user_cmd;
bwang 84:dd32640942a4 83 break;
bwang 84:dd32640942a4 84 case OP_DRIVING:
bwang 84:dd32640942a4 85 control.torque_percent = th->map(control.user_cmd, read.w);
bwang 84:dd32640942a4 86 break;
bwang 84:dd32640942a4 87 case OP_SPEED:
bwang 84:dd32640942a4 88 //set velocity setpoint here;
bwang 84:dd32640942a4 89 break;
bwang 84:dd32640942a4 90 case OP_POSITION:
bwang 84:dd32640942a4 91 //set pos setpoint here;
bwang 84:dd32640942a4 92 break;
bwang 84:dd32640942a4 93 }
bwang 84:dd32640942a4 94
bwang 82:5e741c5ffd9f 95 /*get references*/
bwang 82:5e741c5ffd9f 96 /*happens here, as we may want other controllers running each cycle*/
bwang 82:5e741c5ffd9f 97 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang 82:5e741c5ffd9f 98
bwang 42:030e0ec4eac5 99 /*PI controller*/
bwang 44:3fd6a43b91f0 100 control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
bwang 44:3fd6a43b91f0 101 control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
bwang 42:030e0ec4eac5 102
bwang 42:030e0ec4eac5 103 float d_err = control.d_ref - control.d_filtered;
bwang 42:030e0ec4eac5 104 float q_err = control.q_ref - control.q_filtered;
bwang 42:030e0ec4eac5 105
bwang 58:7316c5a4c417 106 control.d_integral += d_err * KI_D;
bwang 58:7316c5a4c417 107 control.q_integral += q_err * KI_Q;
bwang 42:030e0ec4eac5 108
bwang 64:b4175385d718 109 constrain_norm(&control.d_integral, &control.q_integral, 0.5f, 1.0f, 1.0f);
bwang 42:030e0ec4eac5 110
bwang 58:7316c5a4c417 111 foc.vd = KP_D * d_err + control.d_integral;// - Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f;
bwang 58:7316c5a4c417 112 foc.vq = KP_Q * q_err + control.q_integral;// + Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f;
bwang 42:030e0ec4eac5 113
bwang 64:b4175385d718 114 constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f);
bwang 42:030e0ec4eac5 115
bwang 82:5e741c5ffd9f 116 if (!control.enabled) {
bwang 42:030e0ec4eac5 117 foc.vd = 0.0f;
bwang 42:030e0ec4eac5 118 foc.vq = 0.0f;
bwang 42:030e0ec4eac5 119 }
bwang 42:030e0ec4eac5 120
bwang 42:030e0ec4eac5 121 /*inverse transforms*/
bwang 42:030e0ec4eac5 122 invpark(foc.vd, foc.vq, sin_p, cos_p, &foc.valpha, &foc.vbeta);
bwang 42:030e0ec4eac5 123
bwang 42:030e0ec4eac5 124 float va, vb, vc, voff;
bwang 42:030e0ec4eac5 125
bwang 42:030e0ec4eac5 126 invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang 42:030e0ec4eac5 127 vc = -va - vb;
bwang 42:030e0ec4eac5 128
bwang 42:030e0ec4eac5 129 /*SVPWM*/
bwang 44:3fd6a43b91f0 130 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 42:030e0ec4eac5 131 va = va - voff;
bwang 42:030e0ec4eac5 132 vb = vb - voff;
bwang 42:030e0ec4eac5 133 vc = vc - voff;
bwang 42:030e0ec4eac5 134
bwang 56:c681001dfa46 135 /*safety checks, reset integral*/
bwang 84:dd32640942a4 136 if (!checks_passed()) {
bwang 56:c681001dfa46 137 go_disabled();
bwang 56:c681001dfa46 138 }
bwang 56:c681001dfa46 139
bwang 42:030e0ec4eac5 140 /*output to timers*/
bwang 84:dd32640942a4 141 switch (BREMS_mode) {
bwang 84:dd32640942a4 142 case MODE_CFG:
bwang 84:dd32640942a4 143 set_dtc(io.a, 0.5f);
bwang 84:dd32640942a4 144 set_dtc(io.b, 0.5f);
bwang 84:dd32640942a4 145 set_dtc(io.c, 0.5f);
bwang 84:dd32640942a4 146 break;
bwang 84:dd32640942a4 147 case MODE_RUN:
bwang 84:dd32640942a4 148 set_dtc(io.a, 0.5f + 0.5f * va);
bwang 84:dd32640942a4 149 set_dtc(io.b, 0.5f + 0.5f * vb);
bwang 84:dd32640942a4 150 set_dtc(io.c, 0.5f + 0.5f * vc);
bwang 84:dd32640942a4 151 break;
bwang 84:dd32640942a4 152 case MODE_ZERO:
bwang 84:dd32640942a4 153 //zeroing code here;
bwang 84:dd32640942a4 154 break;
bwang 84:dd32640942a4 155 case MODE_CHR:
bwang 84:dd32640942a4 156 //i have no idea lol;
bwang 84:dd32640942a4 157 break;
bwang 84:dd32640942a4 158 }
bwang 42:030e0ec4eac5 159 }
bwang 42:030e0ec4eac5 160
bwang 44:3fd6a43b91f0 161 void slow_loop() {
bwang 82:5e741c5ffd9f 162 switch (BREMS_src) {
bwang 82:5e741c5ffd9f 163 case CMD_SRC_RC:
bwang 83:eb3704d4943f 164 control.user_cmd = update_filter(control.user_cmd, io.throttle_in->get_throttle(), THROTTLE_FILTER_STRENGTH);
bwang 82:5e741c5ffd9f 165 break;
bwang 82:5e741c5ffd9f 166 case CMD_SRC_ANALOG:
bwang 82:5e741c5ffd9f 167 //analog code here;
bwang 82:5e741c5ffd9f 168 break;
bwang 82:5e741c5ffd9f 169 case CMD_SRC_TERMINAL:
bwang 82:5e741c5ffd9f 170 case CMD_SRC_SERIAL:
bwang 82:5e741c5ffd9f 171 case CMD_SRC_CAN:
bwang 82:5e741c5ffd9f 172 case CMD_SRC_INTERNAL:
bwang 82:5e741c5ffd9f 173 default:
bwang 83:eb3704d4943f 174 //these sources are updated asynchronously via interrupts
bwang 82:5e741c5ffd9f 175 break;
bwang 82:5e741c5ffd9f 176 }
bwang 70:5e39beeb4a21 177 }
bwang 1:7b61790f6be9 178
bwang 0:bac9c3a3a6ca 179 int main() {
bwang 59:0416da6c3060 180 dq = new LutMapper();
bwang 45:cf8ad81fb0f0 181 th = new NullThrottleMapper();
bwang 82:5e741c5ffd9f 182
bwang 82:5e741c5ffd9f 183 BREMSInit(&io, &read, &foc, &control);
bwang 82:5e741c5ffd9f 184 io.pc->attach(rxCallback);
bwang 48:a1a09c83d42c 185
bwang 0:bac9c3a3a6ca 186 for (;;) {
bwang 0:bac9c3a3a6ca 187 }
bwang 42:030e0ec4eac5 188 }