Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

main.cpp@36:cac9785c91cb, 2016-11-27 (annotated)

Committer:
bwang
Date:
Sun Nov 27 09:10:09 2016 +0000
Revision:
36:cac9785c91cb
Parent:
35:89385f64c867
Child:
37:ba7ebf4f8a78
working gokart, stubs for maximum torque per amp based on torque, converting to mpta based on total phase current

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 31:ebe42589ab9d 8 #include "Driving.h"
bwang 31:ebe42589ab9d 9 #include "Optimize.h"
bwang 30:c25c5bf0d951 10
bwang 15:b583cd30b063 11 #include "config_motor.h"
bwang 15:b583cd30b063 12 #include "config_loop.h"
bwang 19:a6cf15f89f3d 13 #include "config_pins.h"
bwang 15:b583cd30b063 14 #include "config_inverter.h"
bwang 29:50e6e4e46580 15 #include "config_driving.h"
bwang 0:bac9c3a3a6ca 16
bwang 1:7b61790f6be9 17 FastPWM *a;
bwang 1:7b61790f6be9 18 FastPWM *b;
bwang 1:7b61790f6be9 19 FastPWM *c;
bwang 0:bac9c3a3a6ca 20 DigitalOut en(EN);
bwang 18:3863ca45cf26 21 PwmIn throttle_in(TH_PIN, 1100, 1900);
bwang 0:bac9c3a3a6ca 22 PositionSensorEncoder pos(CPR, 0);
bwang 0:bac9c3a3a6ca 23
bwang 0:bac9c3a3a6ca 24 Serial pc(USBTX, USBRX);
bwang 0:bac9c3a3a6ca 25
bwang 32:b31423041c4e 26 int adval1, adval2, adval3;
bwang 32:b31423041c4e 27 float vbus = BUS_VOLTAGE;
bwang 2:eabe8feaaabb 28 float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
bwang 30:c25c5bf0d951 29 float p_mech, last_p_mech, w = 0.0f;
bwang 22:72840d3db788 30 float d_filtered = 0.0f, q_filtered = 0.0f;
bwang 2:eabe8feaaabb 31
bwang 1:7b61790f6be9 32 float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV)
bwang 1:7b61790f6be9 33
bwang 2:eabe8feaaabb 34 float d_integral = 0.0f, q_integral = 0.0f;
bwang 2:eabe8feaaabb 35 float last_d = 0.0f, last_q = 0.0f;
bwang 14:59c4fcc1a4f7 36 float d_ref = 0.0f, q_ref = 0.0f;
bwang 2:eabe8feaaabb 37
bwang 25:3f2b585ae72d 38 bool control_enabled = false;
dicarloj 13:41d102a53caf 39
bwang 4:a6669248ce4d 40 void commutate();
bwang 3:9b20da3f0055 41 void zero_current();
bwang 3:9b20da3f0055 42 void config_globals();
bwang 3:9b20da3f0055 43 void startup_msg();
bwang 2:eabe8feaaabb 44
bwang 16:f283d6032fe5 45 void go_enabled();
bwang 16:f283d6032fe5 46 void go_disabled();
dicarloj 13:41d102a53caf 47
bwang 1:7b61790f6be9 48 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
bwang 1:7b61790f6be9 49 if (TIM1->SR & TIM_SR_UIF ) {
bwang 4:a6669248ce4d 50 ADC1->CR2 |= 0x40000000;
bwang 4:a6669248ce4d 51 volatile int delay;
bwang 4:a6669248ce4d 52 for (delay = 0; delay < 35; delay++);
bwang 1:7b61790f6be9 53 adval1 = ADC1->DR;
bwang 1:7b61790f6be9 54 adval2 = ADC2->DR;
bwang 4:a6669248ce4d 55 commutate();
bwang 1:7b61790f6be9 56 }
bwang 1:7b61790f6be9 57 TIM1->SR = 0x00;
bwang 1:7b61790f6be9 58 }
bwang 1:7b61790f6be9 59
bwang 1:7b61790f6be9 60 void zero_current(){
bwang 1:7b61790f6be9 61 for (int i = 0; i < 1000; i++){
bwang 1:7b61790f6be9 62 ia_supp_offset += (float) (ADC1->DR);
bwang 1:7b61790f6be9 63 ib_supp_offset += (float) (ADC2->DR);
bwang 1:7b61790f6be9 64 ADC1->CR2 |= 0x40000000;
bwang 1:7b61790f6be9 65 wait_us(100);
bwang 1:7b61790f6be9 66 }
bwang 1:7b61790f6be9 67 ia_supp_offset /= 1000.0f;
bwang 1:7b61790f6be9 68 ib_supp_offset /= 1000.0f;
bwang 1:7b61790f6be9 69 ia_supp_offset = ia_supp_offset / 4096.0f * AVDD - I_OFFSET;
bwang 1:7b61790f6be9 70 ib_supp_offset = ib_supp_offset / 4096.0f * AVDD - I_OFFSET;
bwang 1:7b61790f6be9 71 }
bwang 0:bac9c3a3a6ca 72
bwang 17:2b852039bb05 73 void update_velocity() {
bwang 15:b583cd30b063 74 last_p_mech = p_mech;
bwang 15:b583cd30b063 75 p_mech = pos.GetMechPosition();
bwang 15:b583cd30b063 76 float dp_mech = p_mech - last_p_mech;
bwang 19:a6cf15f89f3d 77 if (dp_mech < -PI) dp_mech += 2 * PI;
bwang 24:5e18a87a0e95 78 if (dp_mech > PI) dp_mech -= 2 * PI;
bwang 24:5e18a87a0e95 79 float w_raw = dp_mech * F_SW; //rad/s
bwang 29:50e6e4e46580 80 if (w_raw > W_LIMIT) w_raw = w; //with this limiting scheme noise < 0
bwang 29:50e6e4e46580 81 if (w_raw < -W_LIMIT) w_raw = w; //so we need to throw out the large deltas first
bwang 15:b583cd30b063 82 w = W_FILTER_STRENGTH * w + (1.0f - W_FILTER_STRENGTH) * w_raw;
bwang 18:3863ca45cf26 83 }
bwang 18:3863ca45cf26 84
bwang 18:3863ca45cf26 85 float get_torque_cmd(float throttle, float w) {
bwang 30:c25c5bf0d951 86 if (TORQUE_MODE) {
bwang 30:c25c5bf0d951 87 return throttle * FORWARD_TORQUE_MAX;
bwang 30:c25c5bf0d951 88 } else {
bwang 30:c25c5bf0d951 89 return get_driving_torque_cmd(throttle, w);
bwang 30:c25c5bf0d951 90 }
bwang 18:3863ca45cf26 91 }
bwang 18:3863ca45cf26 92
bwang 18:3863ca45cf26 93 //fill in d, q ref based on torque cmd and current velocity
bwang 18:3863ca45cf26 94 void get_dq(float torque, float w, float *d, float *q) {
bwang 36:cac9785c91cb 95 /*
bwang 18:3863ca45cf26 96 *d = 0.0f;
bwang 29:50e6e4e46580 97 *q = torque / KT;
bwang 29:50e6e4e46580 98 if (*q > IPHASE_MOTOR_LIMIT) *q = IPHASE_MOTOR_LIMIT;
bwang 29:50e6e4e46580 99 if (*q > IPHASE_INVERTER_LIMIT) *q = IPHASE_INVERTER_LIMIT;
bwang 36:cac9785c91cb 100 */
bwang 36:cac9785c91cb 101
bwang 31:ebe42589ab9d 102 /*
bwang 31:ebe42589ab9d 103 torque = fabsf(torque);
bwang 32:b31423041c4e 104 get_mtpa_dq(torque, d, q);
bwang 31:ebe42589ab9d 105 if (torque < 0.0f) {
bwang 31:ebe42589ab9d 106 *d = -*d;
bwang 31:ebe42589ab9d 107 *q = -*q;
bwang 31:ebe42589ab9d 108 }
bwang 36:cac9785c91cb 109 */
bwang 36:cac9785c91cb 110
bwang 36:cac9785c91cb 111 float is = torque / KT;
bwang 36:cac9785c91cb 112 *d = (-FLUX_LINKAGE + sqrtf(FLUX_LINKAGE * FLUX_LINKAGE + 8 * (Ld - Lq) * (Ld - Lq) * is * is)) / (4.f * (Ld - Lq));
bwang 36:cac9785c91cb 113 *q = sqrtf(is * is - *d * *d);
bwang 18:3863ca45cf26 114 }
bwang 17:2b852039bb05 115
bwang 17:2b852039bb05 116 void commutate() {
bwang 28:ed9c1ca386fd 117 if (control_enabled && !throttle_in.get_enabled()) go_disabled();
bwang 28:ed9c1ca386fd 118 if (control_enabled && !pos.IsValid()) go_disabled();
bwang 28:ed9c1ca386fd 119 if (!control_enabled && throttle_in.get_enabled()) go_enabled();
bwang 17:2b852039bb05 120
bwang 17:2b852039bb05 121 update_velocity();
bwang 17:2b852039bb05 122
bwang 17:2b852039bb05 123 p = pos.GetElecPosition() - POS_OFFSET;
bwang 15:b583cd30b063 124
bwang 18:3863ca45cf26 125 float torque = get_torque_cmd(throttle_in.get_throttle(), w);
bwang 19:a6cf15f89f3d 126 get_dq(torque, w, &d_ref, &q_ref);
bwang 16:f283d6032fe5 127
bwang 2:eabe8feaaabb 128 float sin_p = sinf(p);
bwang 2:eabe8feaaabb 129 float cos_p = cosf(p);
bwang 2:eabe8feaaabb 130
bwang 1:7b61790f6be9 131 ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE;
bwang 1:7b61790f6be9 132 ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE;
bwang 0:bac9c3a3a6ca 133
bwang 27:7e073122ddf8 134 alpha = ia;
bwang 27:7e073122ddf8 135 beta = 1 / sqrtf(3.0f) * ia + 2 / sqrtf(3.0f) * ib;
bwang 2:eabe8feaaabb 136
bwang 24:5e18a87a0e95 137 d = alpha * cos_p + beta * sin_p;
bwang 24:5e18a87a0e95 138 q = -alpha * sin_p + beta * cos_p;
bwang 2:eabe8feaaabb 139
bwang 22:72840d3db788 140 d_filtered = DQ_FILTER_STRENGTH * d_filtered + (1.0f - DQ_FILTER_STRENGTH) * d;
bwang 22:72840d3db788 141 q_filtered = DQ_FILTER_STRENGTH * q_filtered + (1.0f - DQ_FILTER_STRENGTH) * q;
bwang 22:72840d3db788 142
bwang 22:72840d3db788 143 float d_err = d_ref - d_filtered;
bwang 22:72840d3db788 144 float q_err = q_ref - q_filtered;
bwang 2:eabe8feaaabb 145
bwang 2:eabe8feaaabb 146 d_integral += d_err * KI;
bwang 2:eabe8feaaabb 147 q_integral += q_err * KI;
bwang 2:eabe8feaaabb 148
bwang 20:91ae97a811e3 149 q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
bwang 20:91ae97a811e3 150 d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
bwang 2:eabe8feaaabb 151
bwang 35:89385f64c867 152 vd = KP * d_err + d_integral;// - Lq * POLE_PAIRS * w * q / BUS_VOLTAGE;
bwang 35:89385f64c867 153 vq = KP * q_err + q_integral;// + Ld * POLE_PAIRS * w * d / BUS_VOLTAGE;
bwang 31:ebe42589ab9d 154
bwang 36:cac9785c91cb 155 vd = constrain(vd, -1.0f, 1.0f);
bwang 36:cac9785c91cb 156 vq = constrain(vq, -1.0f, 1.0f);
bwang 21:b7fb355c8c2d 157
bwang 31:ebe42589ab9d 158 float v = sqrtf(vd * vd + vq * vq);
bwang 31:ebe42589ab9d 159
bwang 36:cac9785c91cb 160 /*
bwang 31:ebe42589ab9d 161 if (v > 1.0f) {
bwang 34:454a4256c4fe 162 vq = sqrtf(1.0f - vd * vd);
bwang 31:ebe42589ab9d 163 }
bwang 36:cac9785c91cb 164 */
bwang 4:a6669248ce4d 165
bwang 30:c25c5bf0d951 166 if (!control_enabled) {
bwang 30:c25c5bf0d951 167 vd = 0.0f;
bwang 30:c25c5bf0d951 168 vq = 0.0f;
bwang 30:c25c5bf0d951 169 }
bwang 30:c25c5bf0d951 170
bwang 2:eabe8feaaabb 171 float valpha = vd * cos_p - vq * sin_p;
bwang 2:eabe8feaaabb 172 float vbeta = vd * sin_p + vq * cos_p;
bwang 2:eabe8feaaabb 173
bwang 2:eabe8feaaabb 174 float va = valpha;
bwang 24:5e18a87a0e95 175 float vb = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
bwang 24:5e18a87a0e95 176 float vc = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
bwang 2:eabe8feaaabb 177
dicarloj 13:41d102a53caf 178 float voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;
dicarloj 13:41d102a53caf 179 va = va - voff;
dicarloj 13:41d102a53caf 180 vb = vb - voff;
dicarloj 13:41d102a53caf 181 vc = vc - voff;
dicarloj 13:41d102a53caf 182
bwang 2:eabe8feaaabb 183 set_dtc(a, 0.5f + 0.5f * va);
bwang 2:eabe8feaaabb 184 set_dtc(b, 0.5f + 0.5f * vb);
bwang 2:eabe8feaaabb 185 set_dtc(c, 0.5f + 0.5f * vc);
bwang 0:bac9c3a3a6ca 186 }
bwang 0:bac9c3a3a6ca 187
bwang 0:bac9c3a3a6ca 188 int main() {
bwang 0:bac9c3a3a6ca 189 config_globals();
bwang 0:bac9c3a3a6ca 190 startup_msg();
bwang 0:bac9c3a3a6ca 191
bwang 0:bac9c3a3a6ca 192 for (;;) {
bwang 0:bac9c3a3a6ca 193 }
bwang 0:bac9c3a3a6ca 194 }
bwang 16:f283d6032fe5 195
bwang 16:f283d6032fe5 196 void go_enabled() {
bwang 16:f283d6032fe5 197 d_integral = 0.0f;
bwang 16:f283d6032fe5 198 q_integral = 0.0f;
bwang 16:f283d6032fe5 199 control_enabled = true;
bwang 16:f283d6032fe5 200 en = 1;
bwang 16:f283d6032fe5 201 }
bwang 16:f283d6032fe5 202
bwang 16:f283d6032fe5 203 void go_disabled() {
bwang 16:f283d6032fe5 204 control_enabled = false;
bwang 16:f283d6032fe5 205 en = 0;
bwang 16:f283d6032fe5 206 }
bwang 16:f283d6032fe5 207
bwang 17:2b852039bb05 208 void startup_msg() {
bwang 17:2b852039bb05 209 pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
bwang 17:2b852039bb05 210 pc.printf("%s\n\r", "====Config Data====");
bwang 17:2b852039bb05 211 pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET);
bwang 17:2b852039bb05 212 pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE);
bwang 17:2b852039bb05 213 pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE);
bwang 23:c77d4b42de17 214 pc.printf("Switching Frequency: %f KHz \n\r", F_SW / 1000.0f);
bwang 17:2b852039bb05 215 pc.printf("Pole pairs: %d\n\r", (int) POLE_PAIRS);
bwang 17:2b852039bb05 216 pc.printf("Resolver lobes: %d\n\r", (int) RESOLVER_LOBES);
bwang 17:2b852039bb05 217 pc.printf("Loop KP: %f\n\r", KP);
bwang 17:2b852039bb05 218 pc.printf("Loop KI: %f\n\r", KI);
bwang 17:2b852039bb05 219 pc.printf("Ia offset: %f mV\n\r", ia_supp_offset);
bwang 17:2b852039bb05 220 pc.printf("Ib offset: %f mV\n\r", ib_supp_offset);
bwang 17:2b852039bb05 221 pc.printf("\n\r");
bwang 17:2b852039bb05 222 }
bwang 17:2b852039bb05 223
bwang 17:2b852039bb05 224 void config_globals() {
bwang 17:2b852039bb05 225 pc.baud(115200);
bwang 17:2b852039bb05 226
bwang 17:2b852039bb05 227 //Enable clocks for GPIOs
bwang 17:2b852039bb05 228 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
bwang 17:2b852039bb05 229 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
bwang 17:2b852039bb05 230 RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
bwang 17:2b852039bb05 231
bwang 17:2b852039bb05 232 RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; //enable TIM1 clock
bwang 17:2b852039bb05 233
bwang 17:2b852039bb05 234 a = new FastPWM(PWMA);
bwang 17:2b852039bb05 235 b = new FastPWM(PWMB);
bwang 17:2b852039bb05 236 c = new FastPWM(PWMC);
bwang 17:2b852039bb05 237
bwang 17:2b852039bb05 238 NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ
bwang 17:2b852039bb05 239
bwang 17:2b852039bb05 240 TIM1->DIER |= TIM_DIER_UIE; //enable update interrupt
bwang 17:2b852039bb05 241 TIM1->CR1 = 0x40; //CMS = 10, interrupt only when counting up
bwang 17:2b852039bb05 242 TIM1->CR1 |= TIM_CR1_ARPE; //autoreload on,
bwang 17:2b852039bb05 243 TIM1->RCR |= 0x01; //update event once per up/down count of tim1
bwang 17:2b852039bb05 244 TIM1->EGR |= TIM_EGR_UG;
bwang 17:2b852039bb05 245
bwang 17:2b852039bb05 246 TIM1->PSC = 0x00; //no prescaler, timer counts up in sync with the peripheral clock
bwang 22:72840d3db788 247 TIM1->ARR = (int) ((float) 9e7 / F_SW);
bwang 17:2b852039bb05 248 TIM1->CCER |= ~(TIM_CCER_CC1NP); //Interupt when low side is on.
bwang 17:2b852039bb05 249 TIM1->CR1 |= TIM_CR1_CEN;
bwang 17:2b852039bb05 250
bwang 17:2b852039bb05 251 //ADC Setup
bwang 17:2b852039bb05 252 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1
bwang 17:2b852039bb05 253 RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2
bwang 17:2b852039bb05 254
bwang 17:2b852039bb05 255 ADC->CCR = 0x00000006; //Regular simultaneous mode, 3 channels
bwang 17:2b852039bb05 256
bwang 17:2b852039bb05 257 ADC1->CR2 |= ADC_CR2_ADON; //ADC1 on
bwang 17:2b852039bb05 258 ADC1->SQR3 = 0x0000004; //PA_4 as ADC1, sequence 0
bwang 17:2b852039bb05 259
bwang 17:2b852039bb05 260 ADC2->CR2 |= ADC_CR2_ADON; //ADC2 ON
bwang 17:2b852039bb05 261 ADC2->SQR3 = 0x00000008; //PB_0 as ADC2, sequence 1
bwang 17:2b852039bb05 262
bwang 17:2b852039bb05 263 GPIOA->MODER |= (1 << 8);
bwang 17:2b852039bb05 264 GPIOA->MODER |= (1 << 9);
bwang 17:2b852039bb05 265
bwang 17:2b852039bb05 266 GPIOA->MODER |= (1 << 2);
bwang 17:2b852039bb05 267 GPIOA->MODER |= (1 << 3);
bwang 17:2b852039bb05 268
bwang 17:2b852039bb05 269 GPIOA->MODER |= (1 << 0);
bwang 17:2b852039bb05 270 GPIOA->MODER |= (1 << 1);
bwang 17:2b852039bb05 271
bwang 17:2b852039bb05 272 GPIOB->MODER |= (1 << 0);
bwang 17:2b852039bb05 273 GPIOB->MODER |= (1 << 1);
bwang 17:2b852039bb05 274
bwang 17:2b852039bb05 275 GPIOC->MODER |= (1 << 2);
bwang 17:2b852039bb05 276 GPIOC->MODER |= (1 << 3);
bwang 17:2b852039bb05 277
bwang 17:2b852039bb05 278 //DAC setup
bwang 17:2b852039bb05 279 RCC->APB1ENR |= 0x20000000;
bwang 17:2b852039bb05 280 DAC->CR |= DAC_CR_EN2;
bwang 17:2b852039bb05 281
bwang 17:2b852039bb05 282 GPIOA->MODER |= (1 << 10);
bwang 17:2b852039bb05 283 GPIOA->MODER |= (1 << 11);
bwang 17:2b852039bb05 284
bwang 17:2b852039bb05 285 //Zero duty cycles
bwang 17:2b852039bb05 286 set_dtc(a, 0.0f);
bwang 17:2b852039bb05 287 set_dtc(b, 0.0f);
bwang 17:2b852039bb05 288 set_dtc(c, 0.0f);
bwang 17:2b852039bb05 289
bwang 17:2b852039bb05 290 wait_ms(250);
bwang 17:2b852039bb05 291 zero_current();
bwang 17:2b852039bb05 292 p_mech = pos.GetMechPosition();
bwang 17:2b852039bb05 293 en = 1;
bwang 17:2b852039bb05 294 }