Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@45:cf8ad81fb0f0, 2017-01-08 (annotated)

Committer:
bwang
Date:
Sun Jan 08 09:07:39 2017 +0000
Revision:
45:cf8ad81fb0f0
Parent:
44:3fd6a43b91f0
Child:
46:748aba7d111d
this revision uses the naive (no FW) d/q code and a linear throttle map

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 42:030e0ec4eac5 11
bwang 42:030e0ec4eac5 12 #include "BREMSStructs.h"
bwang 42:030e0ec4eac5 13 #include "BREMSConfig.h"
bwang 30:c25c5bf0d951 14
bwang 15:b583cd30b063 15 #include "config_motor.h"
bwang 15:b583cd30b063 16 #include "config_loop.h"
bwang 19:a6cf15f89f3d 17 #include "config_pins.h"
bwang 15:b583cd30b063 18 #include "config_inverter.h"
bwang 29:50e6e4e46580 19 #include "config_driving.h"
bwang 0:bac9c3a3a6ca 20
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 25:3f2b585ae72d 32 bool control_enabled = false;
dicarloj 13:41d102a53caf 33
bwang 42:030e0ec4eac5 34 void update_velocity() {
bwang 42:030e0ec4eac5 35 read.last_p_mech = read.p_mech;
bwang 42:030e0ec4eac5 36 read.p_mech = io.pos->GetMechPosition();
bwang 44:3fd6a43b91f0 37
bwang 42:030e0ec4eac5 38 float dp_mech = read.p_mech - read.last_p_mech;
bwang 42:030e0ec4eac5 39 if (dp_mech < -PI) dp_mech += 2 * PI;
bwang 42:030e0ec4eac5 40 if (dp_mech > PI) dp_mech -= 2 * PI;
bwang 44:3fd6a43b91f0 41
bwang 42:030e0ec4eac5 42 float w_raw = dp_mech * F_SW; //rad/s
bwang 42:030e0ec4eac5 43 if (w_raw > W_CRAZY) w_raw = read.w; //with this limiting scheme noise < 0
bwang 42:030e0ec4eac5 44 if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first
bwang 44:3fd6a43b91f0 45
bwang 44:3fd6a43b91f0 46 read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH);
bwang 42:030e0ec4eac5 47 }
bwang 2:eabe8feaaabb 48
bwang 42:030e0ec4eac5 49 void commutate() {
bwang 42:030e0ec4eac5 50 /*safety checks*/
bwang 42:030e0ec4eac5 51 if (control_enabled && !io.throttle_in->get_enabled()) go_disabled();
bwang 42:030e0ec4eac5 52 if (control_enabled && !io.pos->IsValid()) go_disabled();
bwang 42:030e0ec4eac5 53 if (!control_enabled && io.throttle_in->get_enabled()) go_enabled();
bwang 42:030e0ec4eac5 54
bwang 42:030e0ec4eac5 55 /*update velocity, references*/
bwang 42:030e0ec4eac5 56 update_velocity();
bwang 44:3fd6a43b91f0 57 if (loop_counter % SLOW_LOOP_COUNTER == 0) {
bwang 44:3fd6a43b91f0 58 loop_counter = 0;
bwang 44:3fd6a43b91f0 59 slow_loop();
bwang 44:3fd6a43b91f0 60 }
bwang 44:3fd6a43b91f0 61 loop_counter++;
bwang 42:030e0ec4eac5 62
bwang 42:030e0ec4eac5 63 /*update position, sin, cos*/
bwang 42:030e0ec4eac5 64 foc.p = io.pos->GetElecPosition() - POS_OFFSET;
bwang 42:030e0ec4eac5 65 float sin_p = sinf(foc.p);
bwang 42:030e0ec4eac5 66 float cos_p = cosf(foc.p);
bwang 42:030e0ec4eac5 67
bwang 42:030e0ec4eac5 68 /*scale and offset currents (adval1, 2 are updated in ISR*/
bwang 42:030e0ec4eac5 69 foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ia_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 70 foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ib_supp_offset) / I_SCALE;
bwang 42:030e0ec4eac5 71
bwang 42:030e0ec4eac5 72 /*compute d, q*/
bwang 42:030e0ec4eac5 73 clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
bwang 42:030e0ec4eac5 74 park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
bwang 42:030e0ec4eac5 75
bwang 42:030e0ec4eac5 76 /*PI controller*/
bwang 44:3fd6a43b91f0 77 control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
bwang 44:3fd6a43b91f0 78 control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
bwang 42:030e0ec4eac5 79
bwang 42:030e0ec4eac5 80 float d_err = control.d_ref - control.d_filtered;
bwang 42:030e0ec4eac5 81 float q_err = control.q_ref - control.q_filtered;
bwang 42:030e0ec4eac5 82
bwang 42:030e0ec4eac5 83 control.d_integral += d_err * KI;
bwang 42:030e0ec4eac5 84 control.q_integral += q_err * KI;
bwang 42:030e0ec4eac5 85
bwang 42:030e0ec4eac5 86 control.q_integral = constrain(control.q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
bwang 42:030e0ec4eac5 87 control.d_integral = constrain(control.d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
bwang 42:030e0ec4eac5 88
bwang 42:030e0ec4eac5 89 foc.vd = KP * d_err + control.d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE;
bwang 42:030e0ec4eac5 90 foc.vq = KP * q_err + control.q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE;
bwang 42:030e0ec4eac5 91
bwang 42:030e0ec4eac5 92 foc.vd = constrain(foc.vd, -1.0f, 1.0f);
bwang 42:030e0ec4eac5 93 foc.vq = constrain(foc.vq, -1.0f, 1.0f);
bwang 42:030e0ec4eac5 94
bwang 42:030e0ec4eac5 95 if (!control_enabled) {
bwang 42:030e0ec4eac5 96 foc.vd = 0.0f;
bwang 42:030e0ec4eac5 97 foc.vq = 0.0f;
bwang 42:030e0ec4eac5 98 }
bwang 42:030e0ec4eac5 99
bwang 42:030e0ec4eac5 100 /*inverse transforms*/
bwang 42:030e0ec4eac5 101 invpark(foc.vd, foc.vq, sin_p, cos_p, &foc.valpha, &foc.vbeta);
bwang 42:030e0ec4eac5 102
bwang 42:030e0ec4eac5 103 float va, vb, vc, voff;
bwang 42:030e0ec4eac5 104
bwang 42:030e0ec4eac5 105 invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang 42:030e0ec4eac5 106 vc = -va - vb;
bwang 42:030e0ec4eac5 107
bwang 42:030e0ec4eac5 108 /*SVPWM*/
bwang 44:3fd6a43b91f0 109 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang 42:030e0ec4eac5 110 va = va - voff;
bwang 42:030e0ec4eac5 111 vb = vb - voff;
bwang 42:030e0ec4eac5 112 vc = vc - voff;
bwang 42:030e0ec4eac5 113
bwang 42:030e0ec4eac5 114 /*output to timers*/
bwang 42:030e0ec4eac5 115 set_dtc(io.a, 0.5f + 0.5f * va);
bwang 42:030e0ec4eac5 116 set_dtc(io.b, 0.5f + 0.5f * vb);
bwang 42:030e0ec4eac5 117 set_dtc(io.c, 0.5f + 0.5f * vc);
bwang 42:030e0ec4eac5 118 }
bwang 42:030e0ec4eac5 119
bwang 44:3fd6a43b91f0 120 void slow_loop() {
bwang 44:3fd6a43b91f0 121 float torque_percent = th->map(io.throttle_in->get_throttle(), read.w);
bwang 44:3fd6a43b91f0 122 dq->map(torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang 44:3fd6a43b91f0 123 }
bwang 44:3fd6a43b91f0 124
bwang 42:030e0ec4eac5 125 void go_enabled() {
bwang 42:030e0ec4eac5 126 control.d_integral = 0.0f;
bwang 42:030e0ec4eac5 127 control.q_integral = 0.0f;
bwang 42:030e0ec4eac5 128 control_enabled = true;
bwang 42:030e0ec4eac5 129 io.en->write(1);
bwang 42:030e0ec4eac5 130 }
bwang 42:030e0ec4eac5 131
bwang 42:030e0ec4eac5 132 void go_disabled() {
bwang 42:030e0ec4eac5 133 control_enabled = false;
bwang 42:030e0ec4eac5 134 io.en->write(0);
bwang 42:030e0ec4eac5 135 }
bwang 42:030e0ec4eac5 136
bwang 44:3fd6a43b91f0 137 float update_filter(float old, float x, float str) {
bwang 42:030e0ec4eac5 138 return str * old + (1.0f - str) * x;
bwang 42:030e0ec4eac5 139 }
dicarloj 13:41d102a53caf 140
bwang 1:7b61790f6be9 141 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
bwang 1:7b61790f6be9 142 if (TIM1->SR & TIM_SR_UIF ) {
bwang 4:a6669248ce4d 143 ADC1->CR2 |= 0x40000000;
bwang 4:a6669248ce4d 144 volatile int delay;
bwang 4:a6669248ce4d 145 for (delay = 0; delay < 35; delay++);
bwang 42:030e0ec4eac5 146 read.adval1 = ADC1->DR;
bwang 42:030e0ec4eac5 147 read.adval2 = ADC2->DR;
bwang 4:a6669248ce4d 148 commutate();
bwang 1:7b61790f6be9 149 }
bwang 1:7b61790f6be9 150 TIM1->SR = 0x00;
bwang 1:7b61790f6be9 151 }
bwang 1:7b61790f6be9 152
bwang 0:bac9c3a3a6ca 153 int main() {
bwang 42:030e0ec4eac5 154 BREMSInit(&io, &read, &foc, &control, false);
bwang 45:cf8ad81fb0f0 155 dq = new LinearNoFWMapper(KT, TORQUE_MAX, FLUX_LINKAGE);
bwang 45:cf8ad81fb0f0 156 th = new NullThrottleMapper();
bwang 0:bac9c3a3a6ca 157
bwang 0:bac9c3a3a6ca 158 for (;;) {
bwang 0:bac9c3a3a6ca 159 }
bwang 42:030e0ec4eac5 160 }