Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@153:8a1f9888f003, 2017-05-04 (annotated)

Committer:
bwang
Date:
Thu May 04 16:14:29 2017 +0000
Revision:
153:8a1f9888f003
Parent:
152:6877dceec871 
temp commit, moving average does not seem to work too well

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 150:08c13bfc7417 33 MedianFilter *throttle_filter, *velocity_filter;
bwang 153:8a1f9888f003 34 MovingAverageFilter *d_filter, *q_filter;
bwang 2:eabe8feaaabb 35
bwang 44:3fd6a43b91f0 36 int loop_counter = 0;
bwang 25:3f2b585ae72d 37 bool control_enabled = false;
dicarloj 13:41d102a53caf 38
bwang 42:030e0ec4eac5 39 void update_velocity() {
bwang 42:030e0ec4eac5 40 read.last_p_mech = read.p_mech;
bwang 42:030e0ec4eac5 41 read.p_mech = io.pos->GetMechPosition();
bwang 44:3fd6a43b91f0 42
bwang 42:030e0ec4eac5 43 float dp_mech = read.p_mech - read.last_p_mech;
bwang 42:030e0ec4eac5 44 if (dp_mech < -PI) dp_mech += 2 * PI;
bwang 42:030e0ec4eac5 45 if (dp_mech > PI) dp_mech -= 2 * PI;
bwang 44:3fd6a43b91f0 46
bwang 42:030e0ec4eac5 47 float w_raw = dp_mech * F_SW; //rad/s
bwang 44:3fd6a43b91f0 48
bwang 152:6877dceec871 49 read.w = velocity_filter->update(w_raw);
bwang 42:030e0ec4eac5 50 }
bwang 2:eabe8feaaabb 51
bwang 118:2b6dab10b69d 52 void commutate() {
bwang 56:c681001dfa46 53 /*safety checks, do we do anything this cycle?*/
bwang 56:c681001dfa46 54 if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
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 130:639cd8586f86 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 42:030e0ec4eac5 79 /*PI controller*/
bwang 153:8a1f9888f003 80 control.d_filtered = d_filter->update(foc.d);
bwang 153:8a1f9888f003 81 control.q_filtered = q_filter->update(foc.q);
bwang 42:030e0ec4eac5 82
bwang 42:030e0ec4eac5 83 float d_err = control.d_ref - control.d_filtered;
bwang 42:030e0ec4eac5 84 float q_err = control.q_ref - control.q_filtered;
bwang 42:030e0ec4eac5 85
bwang 58:7316c5a4c417 86 control.d_integral += d_err * KI_D;
bwang 58:7316c5a4c417 87 control.q_integral += q_err * KI_Q;
bwang 42:030e0ec4eac5 88
bwang 131:031df63c7dbc 89 constrain_norm(&control.d_integral, &control.q_integral, 1.0f, 1.0f, INTEGRAL_MAX);
bwang 131:031df63c7dbc 90
bwang 131:031df63c7dbc 91 foc.vd_decouple = -Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f;
bwang 131:031df63c7dbc 92 foc.vq_decouple = Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f;
bwang 42:030e0ec4eac5 93
bwang 131:031df63c7dbc 94 constrain_norm(&foc.vd_decouple, &foc.vq_decouple, 1.0f, 1.0f, 1.0f);
bwang 131:031df63c7dbc 95
bwang 131:031df63c7dbc 96 foc.vd = KP_D * d_err + control.d_integral;// + foc.vd_decouple;
bwang 131:031df63c7dbc 97 foc.vq = KP_Q * q_err + control.q_integral;// + foc.vq_decouple;
bwang 42:030e0ec4eac5 98
bwang 64:b4175385d718 99 constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f);
bwang 42:030e0ec4eac5 100
bwang 42:030e0ec4eac5 101 if (!control_enabled) {
bwang 42:030e0ec4eac5 102 foc.vd = 0.0f;
bwang 42:030e0ec4eac5 103 foc.vq = 0.0f;
bwang 42:030e0ec4eac5 104 }
bwang 42:030e0ec4eac5 105
bwang 126:498f56ba051e 106 float pv = foc.p + read.w / V_PHASE_SWIZZLE;
bwang 124:e70ca81676fc 107 float sin_pv = sinf(pv);
bwang 124:e70ca81676fc 108 float cos_pv = cosf(pv);
bwang 124:e70ca81676fc 109
bwang 42:030e0ec4eac5 110 /*inverse transforms*/
bwang 124:e70ca81676fc 111 invpark(foc.vd, foc.vq, sin_pv, cos_pv, &foc.valpha, &foc.vbeta);
bwang 42:030e0ec4eac5 112
bwang 42:030e0ec4eac5 113 float va, vb, vc, voff;
bwang 42:030e0ec4eac5 114
bwang 42:030e0ec4eac5 115 invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang 42:030e0ec4eac5 116 vc = -va - vb;
bwang 42:030e0ec4eac5 117
bwang 42:030e0ec4eac5 118 /*SVPWM*/
bwang 44:3fd6a43b91f0 119 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 42:030e0ec4eac5 120 va = va - voff;
bwang 42:030e0ec4eac5 121 vb = vb - voff;
bwang 42:030e0ec4eac5 122 vc = vc - voff;
bwang 42:030e0ec4eac5 123
bwang 56:c681001dfa46 124 /*safety checks, reset integral*/
bwang 56:c681001dfa46 125 if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) {
bwang 56:c681001dfa46 126 /*do this even in disabled state, to keep integral down*/
bwang 56:c681001dfa46 127 go_disabled();
bwang 56:c681001dfa46 128 }
bwang 56:c681001dfa46 129
bwang 42:030e0ec4eac5 130 /*output to timers*/
bwang 93:e7e45957f496 131 set_dtc(io.a, 0.5f + 0.5f * va * MODULATION_MAX);
bwang 93:e7e45957f496 132 set_dtc(io.b, 0.5f + 0.5f * vb * MODULATION_MAX);
bwang 93:e7e45957f496 133 set_dtc(io.c, 0.5f + 0.5f * vc * MODULATION_MAX);
bwang 42:030e0ec4eac5 134 }
bwang 42:030e0ec4eac5 135
bwang 44:3fd6a43b91f0 136 void slow_loop() {
bwang 74:f10cb573d7ca 137 float x = io.throttle_in->get_throttle();
bwang 152:6877dceec871 138 x = throttle_filter->update(x);
bwang 152:6877dceec871 139 control.torque_percent = th->map(x, read.w);
bwang 70:5e39beeb4a21 140 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang 44:3fd6a43b91f0 141 }
bwang 44:3fd6a43b91f0 142
bwang 42:030e0ec4eac5 143 void go_enabled() {
bwang 42:030e0ec4eac5 144 control_enabled = true;
bwang 42:030e0ec4eac5 145 io.en->write(1);
bwang 42:030e0ec4eac5 146 }
bwang 42:030e0ec4eac5 147
bwang 42:030e0ec4eac5 148 void go_disabled() {
bwang 52:fd3d8df99287 149 control.d_integral = 0.0f;
bwang 52:fd3d8df99287 150 control.q_integral = 0.0f;
bwang 42:030e0ec4eac5 151 control_enabled = false;
bwang 42:030e0ec4eac5 152 io.en->write(0);
bwang 42:030e0ec4eac5 153 }
bwang 42:030e0ec4eac5 154
bwang 52:fd3d8df99287 155 bool is_driving() {
bwang 55:3568b78dc203 156 return io.throttle_in->get_throttle() > 0.01f || fabsf(read.w) > W_SAFE;
bwang 52:fd3d8df99287 157 }
bwang 52:fd3d8df99287 158
bwang 44:3fd6a43b91f0 159 float update_filter(float old, float x, float str) {
bwang 42:030e0ec4eac5 160 return str * old + (1.0f - str) * x;
bwang 42:030e0ec4eac5 161 }
dicarloj 13:41d102a53caf 162
bwang 72:5f1da97d62e1 163 void log() {
bwang 153:8a1f9888f003 164 io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent),
bwang 153:8a1f9888f003 165 (int) (255 * foc.vd), (int) (255 * foc.vq));
bwang 143:a7a0c9d70e8a 166 //io.pc->printf("%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_filtered, (int) control.q_filtered, (int) (255 * foc.vd), (int) (255 * foc.vq));
bwang 72:5f1da97d62e1 167 wait(1.0f / LOG_FREQUENCY);
bwang 70:5e39beeb4a21 168 }
bwang 70:5e39beeb4a21 169
bwang 1:7b61790f6be9 170 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
bwang 75:591556ce033d 171 int start_state = io.throttle_in->state();
bwang 91:f58472ac3fae 172
bwang 1:7b61790f6be9 173 if (TIM1->SR & TIM_SR_UIF ) {
bwang 4:a6669248ce4d 174 ADC1->CR2 |= 0x40000000;
bwang 91:f58472ac3fae 175 volatile int delay;
bwang 91:f58472ac3fae 176 for (delay = 0; delay < 35; delay++);
bwang 91:f58472ac3fae 177
bwang 42:030e0ec4eac5 178 read.adval1 = ADC1->DR;
bwang 42:030e0ec4eac5 179 read.adval2 = ADC2->DR;
bwang 91:f58472ac3fae 180
bwang 4:a6669248ce4d 181 commutate();
bwang 1:7b61790f6be9 182 }
bwang 1:7b61790f6be9 183 TIM1->SR = 0x00;
bwang 75:591556ce033d 184 int end_state = io.throttle_in->state();
bwang 75:591556ce033d 185 if (start_state != end_state) io.throttle_in->block();
bwang 1:7b61790f6be9 186 }
bwang 1:7b61790f6be9 187
bwang 0:bac9c3a3a6ca 188 int main() {
bwang 132:101b74e4763a 189 dq = new LutMapper();
bwang 45:cf8ad81fb0f0 190 th = new NullThrottleMapper();
bwang 153:8a1f9888f003 191
bwang 152:6877dceec871 192 throttle_filter = new MedianFilter(THROTTLE_FILTER_WINDOW);
bwang 152:6877dceec871 193 velocity_filter = new MedianFilter(W_FILTER_WINDOW);
bwang 153:8a1f9888f003 194 d_filter = new MovingAverageFilter(DQ_FILTER_WINDOW);
bwang 153:8a1f9888f003 195 q_filter = new MovingAverageFilter(DQ_FILTER_WINDOW);
bwang 150:08c13bfc7417 196
bwang 47:1c9868e226d0 197 BREMSInit(&io, &read, &foc, &control, false);
bwang 48:a1a09c83d42c 198
bwang 0:bac9c3a3a6ca 199 for (;;) {
bwang 72:5f1da97d62e1 200 if (ENABLE_LOGGING) {
bwang 72:5f1da97d62e1 201 log();
bwang 72:5f1da97d62e1 202 }
bwang 0:bac9c3a3a6ca 203 }
bwang 42:030e0ec4eac5 204 }