Bayley Wang / foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Committer:
bwang
Date:
Thu Apr 06 17:33:47 2017 +0000
Revision:
92:a9dac72d8cac
Parent:
91:f58472ac3fae
Child:
93:e7e45957f496
--PwmIn now checks lower bounds for sanity and fall without rise; --switched to edge aligned pwm to work around prius module propagation delays

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang0:bac9c3a3a6ca 1#include "mbed.h"
bwang0:bac9c3a3a6ca 2#include "math.h"
bwang19:a6cf15f89f3d 3
bwang0:bac9c3a3a6ca 4#include "PositionSensor.h"
bwang0:bac9c3a3a6ca 5#include "FastPWM.h"
bwang18:3863ca45cf26 6#include "PwmIn.h"
bwang19:a6cf15f89f3d 7#include "MathHelpers.h"
bwang42:030e0ec4eac5 8#include "Transforms.h"
bwang42:030e0ec4eac5 9#include "DQMapper.h"
bwang42:030e0ec4eac5 10#include "ThrottleMapper.h"
bwang42:030e0ec4eac5 11
bwang42:030e0ec4eac5 12#include "BREMSStructs.h"
bwang42:030e0ec4eac5 13#include "BREMSConfig.h"
bwang30:c25c5bf0d951 14
bwang15:b583cd30b063 15#include "config_motor.h"
bwang15:b583cd30b063 16#include "config_loop.h"
bwang19:a6cf15f89f3d 17#include "config_pins.h"
bwang15:b583cd30b063 18#include "config_inverter.h"
bwang29:50e6e4e46580 19#include "config_driving.h"
bwang72:5f1da97d62e1 20#include "config_logging.h"
bwang0:bac9c3a3a6ca 21
bwang42:030e0ec4eac5 22#include "main.h"
bwang0:bac9c3a3a6ca 23
bwang42:030e0ec4eac5 24IOStruct io;
bwang42:030e0ec4eac5 25ReadDataStruct read;
bwang42:030e0ec4eac5 26FOCStruct foc;
bwang42:030e0ec4eac5 27ControlStruct control;
bwang0:bac9c3a3a6ca 28
bwang42:030e0ec4eac5 29DQMapper *dq;
bwang42:030e0ec4eac5 30ThrottleMapper *th;
bwang2:eabe8feaaabb 31
bwang44:3fd6a43b91f0 32int loop_counter = 0;
bwang25:3f2b585ae72d 33bool control_enabled = false;
bwang90:2ef53b1a22de 34DigitalOut test(PC_5);
dicarloj13:41d102a53caf 35
bwang42:030e0ec4eac5 36void update_velocity() {
bwang42:030e0ec4eac5 37 read.last_p_mech = read.p_mech;
bwang42:030e0ec4eac5 38 read.p_mech = io.pos->GetMechPosition();
bwang44:3fd6a43b91f0 39
bwang42:030e0ec4eac5 40 float dp_mech = read.p_mech - read.last_p_mech;
bwang42:030e0ec4eac5 41 if (dp_mech < -PI) dp_mech += 2 * PI;
bwang42:030e0ec4eac5 42 if (dp_mech > PI) dp_mech -= 2 * PI;
bwang44:3fd6a43b91f0 43
bwang42:030e0ec4eac5 44 float w_raw = dp_mech * F_SW; //rad/s
bwang42:030e0ec4eac5 45 if (w_raw > W_CRAZY) w_raw = read.w; //with this limiting scheme noise < 0
bwang42:030e0ec4eac5 46 if (w_raw < -W_CRAZY) w_raw = read.w; //so we need to throw out the large deltas first
bwang44:3fd6a43b91f0 47
bwang44:3fd6a43b91f0 48 read.w = update_filter(read.w, w_raw, W_FILTER_STRENGTH);
bwang42:030e0ec4eac5 49}
bwang2:eabe8feaaabb 50
bwang75:591556ce033d 51void commutate() {
bwang56:c681001dfa46 52 /*safety checks, do we do anything this cycle?*/
bwang56:c681001dfa46 53 if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
bwang52:fd3d8df99287 54 go_enabled();
bwang52:fd3d8df99287 55 }
bwang42:030e0ec4eac5 56
bwang42:030e0ec4eac5 57 /*update velocity, references*/
bwang42:030e0ec4eac5 58 update_velocity();
bwang44:3fd6a43b91f0 59 if (loop_counter % SLOW_LOOP_COUNTER == 0) {
bwang44:3fd6a43b91f0 60 loop_counter = 0;
bwang44:3fd6a43b91f0 61 slow_loop();
bwang44:3fd6a43b91f0 62 }
bwang44:3fd6a43b91f0 63 loop_counter++;
bwang42:030e0ec4eac5 64
bwang42:030e0ec4eac5 65 /*update position, sin, cos*/
bwang42:030e0ec4eac5 66 foc.p = io.pos->GetElecPosition() - POS_OFFSET;
bwang42:030e0ec4eac5 67 float sin_p = sinf(foc.p);
bwang42:030e0ec4eac5 68 float cos_p = cosf(foc.p);
bwang42:030e0ec4eac5 69
bwang49:da8604278d76 70 /*scale and offset currents (adval1, 2 are updated in ISR)*/
bwang42:030e0ec4eac5 71 foc.ia = ((float) read.adval1 / 4096.0f * AVDD - I_OFFSET - read.ia_supp_offset) / I_SCALE;
bwang42:030e0ec4eac5 72 foc.ib = ((float) read.adval2 / 4096.0f * AVDD - I_OFFSET - read.ib_supp_offset) / I_SCALE;
bwang42:030e0ec4eac5 73
bwang42:030e0ec4eac5 74 /*compute d, q*/
bwang42:030e0ec4eac5 75 clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
bwang42:030e0ec4eac5 76 park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
bwang42:030e0ec4eac5 77
bwang42:030e0ec4eac5 78 /*PI controller*/
bwang44:3fd6a43b91f0 79 control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
bwang44:3fd6a43b91f0 80 control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
bwang42:030e0ec4eac5 81
bwang42:030e0ec4eac5 82 float d_err = control.d_ref - control.d_filtered;
bwang42:030e0ec4eac5 83 float q_err = control.q_ref - control.q_filtered;
bwang42:030e0ec4eac5 84
bwang58:7316c5a4c417 85 control.d_integral += d_err * KI_D;
bwang58:7316c5a4c417 86 control.q_integral += q_err * KI_Q;
bwang42:030e0ec4eac5 87
bwang90:2ef53b1a22de 88 constrain_norm(&control.d_integral, &control.q_integral, 0.5f, 1.0f, 1.0f);
bwang42:030e0ec4eac5 89
bwang58:7316c5a4c417 90 foc.vd = KP_D * d_err + control.d_integral;// - Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f;
bwang58:7316c5a4c417 91 foc.vq = KP_Q * q_err + control.q_integral;// + Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f;
bwang42:030e0ec4eac5 92
bwang64:b4175385d718 93 constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f);
bwang42:030e0ec4eac5 94
bwang42:030e0ec4eac5 95 if (!control_enabled) {
bwang42:030e0ec4eac5 96 foc.vd = 0.0f;
bwang42:030e0ec4eac5 97 foc.vq = 0.0f;
bwang42:030e0ec4eac5 98 }
bwang42:030e0ec4eac5 99
bwang42:030e0ec4eac5 100 /*inverse transforms*/
bwang42:030e0ec4eac5 101 invpark(foc.vd, foc.vq, sin_p, cos_p, &foc.valpha, &foc.vbeta);
bwang42:030e0ec4eac5 102
bwang42:030e0ec4eac5 103 float va, vb, vc, voff;
bwang42:030e0ec4eac5 104
bwang42:030e0ec4eac5 105 invclarke(foc.valpha, foc.vbeta, &va, &vb);
bwang42:030e0ec4eac5 106 vc = -va - vb;
bwang42:030e0ec4eac5 107
bwang42:030e0ec4eac5 108 /*SVPWM*/
bwang44:3fd6a43b91f0 109 voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it
bwang42:030e0ec4eac5 110 va = va - voff;
bwang42:030e0ec4eac5 111 vb = vb - voff;
bwang42:030e0ec4eac5 112 vc = vc - voff;
bwang42:030e0ec4eac5 113
bwang56:c681001dfa46 114 /*safety checks, reset integral*/
bwang56:c681001dfa46 115 if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) {
bwang56:c681001dfa46 116 /*do this even in disabled state, to keep integral down*/
bwang56:c681001dfa46 117 go_disabled();
bwang56:c681001dfa46 118 }
bwang56:c681001dfa46 119
bwang42:030e0ec4eac5 120 /*output to timers*/
bwang92:a9dac72d8cac 121 set_dtc(io.a, 0.5f + 0.5f * va * 0.89f);
bwang92:a9dac72d8cac 122 set_dtc(io.b, 0.5f + 0.5f * vb * 0.89f);
bwang92:a9dac72d8cac 123 set_dtc(io.c, 0.5f + 0.5f * vc * 0.89f);
bwang42:030e0ec4eac5 124}
bwang42:030e0ec4eac5 125
bwang44:3fd6a43b91f0 126void slow_loop() {
bwang74:f10cb573d7ca 127 float x = io.throttle_in->get_throttle();
bwang75:591556ce033d 128 control.torque_percent = update_filter(control.torque_percent, th->map(io.throttle_in->get_throttle(), read.w), THROTTLE_FILTER_STRENGTH);
bwang75:591556ce033d 129
bwang70:5e39beeb4a21 130 dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
bwang44:3fd6a43b91f0 131}
bwang44:3fd6a43b91f0 132
bwang42:030e0ec4eac5 133void go_enabled() {
bwang42:030e0ec4eac5 134 control_enabled = true;
bwang42:030e0ec4eac5 135 io.en->write(1);
bwang42:030e0ec4eac5 136}
bwang42:030e0ec4eac5 137
bwang42:030e0ec4eac5 138void go_disabled() {
bwang52:fd3d8df99287 139 control.d_integral = 0.0f;
bwang52:fd3d8df99287 140 control.q_integral = 0.0f;
bwang42:030e0ec4eac5 141 control_enabled = false;
bwang42:030e0ec4eac5 142 io.en->write(0);
bwang42:030e0ec4eac5 143}
bwang42:030e0ec4eac5 144
bwang52:fd3d8df99287 145bool is_driving() {
bwang55:3568b78dc203 146 return io.throttle_in->get_throttle() > 0.01f || fabsf(read.w) > W_SAFE;
bwang52:fd3d8df99287 147}
bwang52:fd3d8df99287 148
bwang44:3fd6a43b91f0 149float update_filter(float old, float x, float str) {
bwang42:030e0ec4eac5 150 return str * old + (1.0f - str) * x;
bwang42:030e0ec4eac5 151}
dicarloj13:41d102a53caf 152
bwang72:5f1da97d62e1 153void log() {
bwang92:a9dac72d8cac 154 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),
bwang92:a9dac72d8cac 155 (int) (255 * foc.vd), (int) (255 * foc.vq));
bwang92:a9dac72d8cac 156 //io.pc->printf("%d\n", (int) (255 * control.torque_percent));
bwang72:5f1da97d62e1 157 wait(1.0f / LOG_FREQUENCY);
bwang70:5e39beeb4a21 158}
bwang70:5e39beeb4a21 159
bwang1:7b61790f6be9 160extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
bwang75:591556ce033d 161 int start_state = io.throttle_in->state();
bwang91:f58472ac3fae 162
bwang1:7b61790f6be9 163 if (TIM1->SR & TIM_SR_UIF ) {
bwang91:f58472ac3fae 164 test = 1;
bwang4:a6669248ce4d 165 ADC1->CR2 |= 0x40000000;
bwang91:f58472ac3fae 166 volatile int delay;
bwang91:f58472ac3fae 167 for (delay = 0; delay < 35; delay++);
bwang91:f58472ac3fae 168
bwang42:030e0ec4eac5 169 read.adval1 = ADC1->DR;
bwang42:030e0ec4eac5 170 read.adval2 = ADC2->DR;
bwang91:f58472ac3fae 171 test = 0;
bwang91:f58472ac3fae 172
bwang4:a6669248ce4d 173 commutate();
bwang1:7b61790f6be9 174 }
bwang1:7b61790f6be9 175 TIM1->SR = 0x00;
bwang75:591556ce033d 176 int end_state = io.throttle_in->state();
bwang75:591556ce033d 177 if (start_state != end_state) io.throttle_in->block();
bwang1:7b61790f6be9 178}
bwang1:7b61790f6be9 179
bwang0:bac9c3a3a6ca 180int main() {
bwang59:0416da6c3060 181 dq = new LutMapper();
bwang45:cf8ad81fb0f0 182 th = new NullThrottleMapper();
bwang47:1c9868e226d0 183 BREMSInit(&io, &read, &foc, &control, false);
bwang48:a1a09c83d42c 184
bwang0:bac9c3a3a6ca 185 for (;;) {
bwang72:5f1da97d62e1 186 if (ENABLE_LOGGING) {
bwang72:5f1da97d62e1 187 log();
bwang72:5f1da97d62e1 188 }
bwang0:bac9c3a3a6ca 189 }
bwang42:030e0ec4eac5 190}