Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@180:a783a972a867, 2018-02-09 (annotated)

Committer:
bwang
Date:
Fri Feb 09 00:48:26 2018 +0000
Revision:
180:a783a972a867
Parent:
174:3872516b0d04
Child:
181:d3510c8beab6
Added BufferedLogger object to IOStruct, logging seems to work (insofar as data is sent over serial and the motor still seems to spin)

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 42:030e0ec4eac5 13
bwang 42:030e0ec4eac5 14 #include "BREMSStructs.h"
bwang 42:030e0ec4eac5 15 #include "BREMSConfig.h"
bwang 30:c25c5bf0d951 16
bwang 15:b583cd30b063 17 #include "config_motor.h"
bwang 15:b583cd30b063 18 #include "config_loop.h"
bwang 19:a6cf15f89f3d 19 #include "config_pins.h"
bwang 15:b583cd30b063 20 #include "config_inverter.h"
bwang 29:50e6e4e46580 21 #include "config_driving.h"
bwang 72:5f1da97d62e1 22 #include "config_logging.h"
bwang 0:bac9c3a3a6ca 23
bwang 42:030e0ec4eac5 24 #include "main.h"
bwang 0:bac9c3a3a6ca 25
bwang 42:030e0ec4eac5 26 IOStruct io;
bwang 42:030e0ec4eac5 27 ReadDataStruct read;
bwang 42:030e0ec4eac5 28 FOCStruct foc;
bwang 42:030e0ec4eac5 29 ControlStruct control;
bwang 2:eabe8feaaabb 30
bwang 42:030e0ec4eac5 31 DQMapper *dq;
bwang 42:030e0ec4eac5 32 ThrottleMapper *th;
bwang 2:eabe8feaaabb 33
bwang 44:3fd6a43b91f0 34 int loop_counter = 0;
bwang 25:3f2b585ae72d 35 bool control_enabled = false;
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 44:3fd6a43b91f0 46
bwang 154:0a22dcf91577 47 read.w = control.velocity_filter->update(w_raw);
bwang 42:030e0ec4eac5 48 }
bwang 2:eabe8feaaabb 49
bwang 118:2b6dab10b69d 50 void commutate() {
bwang 56:c681001dfa46 51 /*safety checks, do we do anything this cycle?*/
bwang 56:c681001dfa46 52 if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
bwang 52:fd3d8df99287 53 go_enabled();
bwang 52:fd3d8df99287 54 }
bwang 42:030e0ec4eac5 55
bwang 42:030e0ec4eac5 56 /*update velocity, references*/
bwang 42:030e0ec4eac5 57 update_velocity();
bwang 44:3fd6a43b91f0 58 if (loop_counter % SLOW_LOOP_COUNTER == 0) {
bwang 44:3fd6a43b91f0 59 loop_counter = 0;
bwang 44:3fd6a43b91f0 60 slow_loop();
bwang 44:3fd6a43b91f0 61 }
bwang 44:3fd6a43b91f0 62 loop_counter++;
bwang 42:030e0ec4eac5 63
bwang 42:030e0ec4eac5 64 /*update position, sin, cos*/
bwang 130:639cd8586f86 65 foc.p = io.pos->GetElecPosition() - POS_OFFSET;
bwang 42:030e0ec4eac5 66 float sin_p = sinf(foc.p);
bwang 42:030e0ec4eac5 67 float cos_p = cosf(foc.p);
bwang 42:030e0ec4eac5 68
bwang 49:da8604278d76 69 /*scale and offset currents (adval1, 2 are updated in ISR)*/
bwang 42:030e0ec4eac5 70 foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ia_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 71 foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ib_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 72
bwang 42:030e0ec4eac5 73 /*compute d, q*/
bwang 42:030e0ec4eac5 74 clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
bwang 42:030e0ec4eac5 75 park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
bwang 42:030e0ec4eac5 76
bwang 42:030e0ec4eac5 77 /*PI controller*/
bwang 44:3fd6a43b91f0 78 control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
bwang 44:3fd6a43b91f0 79 control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
bwang 42:030e0ec4eac5 80
bwang 42:030e0ec4eac5 81 float d_err = control.d_ref - control.d_filtered;
bwang 42:030e0ec4eac5 82 float q_err = control.q_ref - control.q_filtered;
bwang 42:030e0ec4eac5 83
bwang 58:7316c5a4c417 84 control.d_integral += d_err * KI_D;
bwang 58:7316c5a4c417 85 control.q_integral += q_err * KI_Q;
bwang 42:030e0ec4eac5 86
bwang 131:031df63c7dbc 87 constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, INTEGRAL_MAX);
bwang 131:031df63c7dbc 88
bwang 131:031df63c7dbc 89 foc.vd_decouple = -Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f;
bwang 131:031df63c7dbc 90 foc.vq_decouple = Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f;
bwang 42:030e0ec4eac5 91
bwang 158:882f9c208378 92 constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f);
bwang 131:031df63c7dbc 93
bwang 131:031df63c7dbc 94 foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple;
bwang 131:031df63c7dbc 95 foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple;
bwang 42:030e0ec4eac5 96
bwang 158:882f9c208378 97 constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f + OVERMODULATION_FACTOR);
bwang 42:030e0ec4eac5 98
bwang 42:030e0ec4eac5 99 if (!control_enabled) {
bwang 42:030e0ec4eac5 100 foc.vd = 0.0f;
bwang 42:030e0ec4eac5 101 foc.vq = 0.0f;
bwang 42:030e0ec4eac5 102 }
bwang 42:030e0ec4eac5 103
bwang 126:498f56ba051e 104 float pv = foc.p + read.w / V_PHASE_SWIZZLE;
bwang 124:e70ca81676fc 105 float sin_pv = sinf(pv);
bwang 124:e70ca81676fc 106 float cos_pv = cosf(pv);
bwang 124:e70ca81676fc 107
bwang 42:030e0ec4eac5 108 /*inverse transforms*/
bwang 124:e70ca81676fc 109 invpark(foc.vd, foc.vq, sin_pv, cos_pv, &foc.valpha, &foc.vbeta);
bwang 42:030e0ec4eac5 110
bwang 42:030e0ec4eac5 111 float va, vb, vc, voff;
bwang 42:030e0ec4eac5 112
bwang 42:030e0ec4eac5 113 invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang 42:030e0ec4eac5 114 vc = -va - vb;
bwang 42:030e0ec4eac5 115
bwang 42:030e0ec4eac5 116 /*SVPWM*/
bwang 44:3fd6a43b91f0 117 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 42:030e0ec4eac5 118 va = va - voff;
bwang 42:030e0ec4eac5 119 vb = vb - voff;
bwang 42:030e0ec4eac5 120 vc = vc - voff;
bwang 42:030e0ec4eac5 121
bwang 56:c681001dfa46 122 /*safety checks, reset integral*/
bwang 56:c681001dfa46 123 if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) {
bwang 56:c681001dfa46 124 /*do this even in disabled state, to keep integral down*/
bwang 56:c681001dfa46 125 go_disabled();
bwang 56:c681001dfa46 126 }
bwang 56:c681001dfa46 127
bwang 42:030e0ec4eac5 128 /*output to timers*/
bwang 157:a9b2002994d5 129 set_dtc(io.a, 0.5f + 0.5f * va * LINEAR_MODULATION_MAX);
bwang 157:a9b2002994d5 130 set_dtc(io.b, 0.5f + 0.5f * vb * LINEAR_MODULATION_MAX);
bwang 157:a9b2002994d5 131 set_dtc(io.c, 0.5f + 0.5f * vc * LINEAR_MODULATION_MAX);
bwang 180:a783a972a867 132
bwang 180:a783a972a867 133 /*log data*/
bwang 180:a783a972a867 134 if (ENABLE_LOGGING) log();
bwang 42:030e0ec4eac5 135 }
bwang 42:030e0ec4eac5 136
bwang 44:3fd6a43b91f0 137 void slow_loop() {
bwang 74:f10cb573d7ca 138 float x = io.throttle_in->get_throttle();
bwang 154:0a22dcf91577 139 x = control.throttle_filter->update(x);
bwang 152:6877dceec871 140 control.torque_percent = th->map(x, read.w);
bwang 70:5e39beeb4a21 141 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang 44:3fd6a43b91f0 142 }
bwang 44:3fd6a43b91f0 143
bwang 42:030e0ec4eac5 144 void go_enabled() {
bwang 42:030e0ec4eac5 145 control_enabled = true;
bwang 42:030e0ec4eac5 146 io.en->write(1);
bwang 42:030e0ec4eac5 147 }
bwang 42:030e0ec4eac5 148
bwang 42:030e0ec4eac5 149 void go_disabled() {
bwang 52:fd3d8df99287 150 control.d_integral = 0.0f;
bwang 52:fd3d8df99287 151 control.q_integral = 0.0f;
bwang 42:030e0ec4eac5 152 control_enabled = false;
bwang 42:030e0ec4eac5 153 io.en->write(0);
bwang 42:030e0ec4eac5 154 }
bwang 42:030e0ec4eac5 155
bwang 52:fd3d8df99287 156 bool is_driving() {
bwang 154:0a22dcf91577 157 return control.torque_percent > 0.01f || fabsf(read.w) > W_SAFE;
bwang 52:fd3d8df99287 158 }
bwang 52:fd3d8df99287 159
bwang 44:3fd6a43b91f0 160 float update_filter(float old, float x, float str) {
bwang 42:030e0ec4eac5 161 return str * old + (1.0f - str) * x;
bwang 42:030e0ec4eac5 162 }
dicarloj 13:41d102a53caf 163
bwang 72:5f1da97d62e1 164 void log() {
bwang 180:a783a972a867 165 float packet[8];
bwang 180:a783a972a867 166 packet[0] = read.w / 8.0f;
bwang 180:a783a972a867 167 packet[1] = control.d_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 168 packet[2] = control.d_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 169 packet[3] = control.q_ref / 2.0f + 128.0f;
bwang 180:a783a972a867 170 packet[4] = control.q_filtered / 2.0f + 128.0f;
bwang 180:a783a972a867 171 packet[5] = 255.0f * control.torque_percent;
bwang 180:a783a972a867 172 packet[6] = 255.0f / (1.0f + OVERMODULATION_FACTOR) * foc.vd / 2.0f + 128.0f;
bwang 180:a783a972a867 173 packet[7] = 255.0f / (1.0f + OVERMODULATION_FACTOR) * foc.vq / 2.0f + 128.0f;
bwang 180:a783a972a867 174 io.logger->log(packet);
bwang 70:5e39beeb4a21 175 }
bwang 70:5e39beeb4a21 176
bwang 1:7b61790f6be9 177 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
bwang 75:591556ce033d 178 int start_state = io.throttle_in->state();
bwang 91:f58472ac3fae 179
bwang 1:7b61790f6be9 180 if (TIM1->SR & TIM_SR_UIF ) {
bwang 4:a6669248ce4d 181 ADC1->CR2 |= 0x40000000;
bwang 91:f58472ac3fae 182 volatile int delay;
bwang 91:f58472ac3fae 183 for (delay = 0; delay < 35; delay++);
bwang 91:f58472ac3fae 184
bwang 42:030e0ec4eac5 185 read.adval1 = ADC1->DR;
bwang 42:030e0ec4eac5 186 read.adval2 = ADC2->DR;
bwang 91:f58472ac3fae 187
bwang 4:a6669248ce4d 188 commutate();
bwang 1:7b61790f6be9 189 }
bwang 1:7b61790f6be9 190 TIM1->SR = 0x00;
bwang 75:591556ce033d 191 int end_state = io.throttle_in->state();
bwang 75:591556ce033d 192 if (start_state != end_state) io.throttle_in->block();
bwang 1:7b61790f6be9 193 }
bwang 1:7b61790f6be9 194
bwang 166:4637785ba01e 195 int main() {
bwang 172:3d7196b71afd 196 dq = new InterpolatingLutMapper();
bwang 174:3872516b0d04 197 th = new LimitingThrottleMapper(1500.0f);
bwang 180:a783a972a867 198
bwang 47:1c9868e226d0 199 BREMSInit(&io, &read, &foc, &control, false);
bwang 48:a1a09c83d42c 200
bwang 0:bac9c3a3a6ca 201 for (;;) {
bwang 180:a783a972a867 202 io.logger->flush();
bwang 0:bac9c3a3a6ca 203 }
bwang 42:030e0ec4eac5 204 }