Bayley Wang
/
foc-ed_in_the_bot_compact
robot
main.cpp@71:292dfc44ca53, 2017-02-05 (annotated)
- Committer:
- bwang
- Date:
- Sun Feb 05 11:58:06 2017 +0000
- Revision:
- 71:292dfc44ca53
- Parent:
- 70:5e39beeb4a21
- Child:
- 72:5f1da97d62e1
throttle filtering (0.9) + debug statements
Who changed what in which revision?
User | Revision | Line number | New 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 | 56:c681001dfa46 | 50 | /*safety checks, do we do anything this cycle?*/ |
bwang | 56:c681001dfa46 | 51 | if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) { |
bwang | 52:fd3d8df99287 | 52 | go_enabled(); |
bwang | 52:fd3d8df99287 | 53 | } |
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 | 49:da8604278d76 | 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 | 58:7316c5a4c417 | 83 | control.d_integral += d_err * KI_D; |
bwang | 58:7316c5a4c417 | 84 | control.q_integral += q_err * KI_Q; |
bwang | 42:030e0ec4eac5 | 85 | |
bwang | 64:b4175385d718 | 86 | constrain_norm(&control.d_integral, &control.q_integral, 0.5f, 1.0f, 1.0f); |
bwang | 42:030e0ec4eac5 | 87 | |
bwang | 58:7316c5a4c417 | 88 | foc.vd = KP_D * d_err + control.d_integral;// - Lq * POLE_PAIRS * read.w * foc.q / BUS_VOLTAGE / 2.0f; |
bwang | 58:7316c5a4c417 | 89 | foc.vq = KP_Q * q_err + control.q_integral;// + Ld * POLE_PAIRS * read.w * foc.d / BUS_VOLTAGE / 2.0f; |
bwang | 42:030e0ec4eac5 | 90 | |
bwang | 64:b4175385d718 | 91 | constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f); |
bwang | 42:030e0ec4eac5 | 92 | |
bwang | 42:030e0ec4eac5 | 93 | if (!control_enabled) { |
bwang | 42:030e0ec4eac5 | 94 | foc.vd = 0.0f; |
bwang | 42:030e0ec4eac5 | 95 | foc.vq = 0.0f; |
bwang | 42:030e0ec4eac5 | 96 | } |
bwang | 42:030e0ec4eac5 | 97 | |
bwang | 42:030e0ec4eac5 | 98 | /*inverse transforms*/ |
bwang | 42:030e0ec4eac5 | 99 | invpark(foc.vd, foc.vq, sin_p, cos_p, &foc.valpha, &foc.vbeta); |
bwang | 42:030e0ec4eac5 | 100 | |
bwang | 42:030e0ec4eac5 | 101 | float va, vb, vc, voff; |
bwang | 42:030e0ec4eac5 | 102 | |
bwang | 42:030e0ec4eac5 | 103 | invclarke(foc.valpha, foc.vbeta, &va, &vb); |
bwang | 42:030e0ec4eac5 | 104 | vc = -va - vb; |
bwang | 42:030e0ec4eac5 | 105 | |
bwang | 42:030e0ec4eac5 | 106 | /*SVPWM*/ |
bwang | 44:3fd6a43b91f0 | 107 | voff = (fminf(va, fminf(vb, vc)) + fmaxf(va, fmaxf(vb, vc)))/2.0f;//don't think about it |
bwang | 42:030e0ec4eac5 | 108 | va = va - voff; |
bwang | 42:030e0ec4eac5 | 109 | vb = vb - voff; |
bwang | 42:030e0ec4eac5 | 110 | vc = vc - voff; |
bwang | 42:030e0ec4eac5 | 111 | |
bwang | 56:c681001dfa46 | 112 | /*safety checks, reset integral*/ |
bwang | 56:c681001dfa46 | 113 | if (!io.throttle_in->get_enabled() || !io.pos->IsValid() || !is_driving()) { |
bwang | 56:c681001dfa46 | 114 | /*do this even in disabled state, to keep integral down*/ |
bwang | 56:c681001dfa46 | 115 | go_disabled(); |
bwang | 56:c681001dfa46 | 116 | } |
bwang | 56:c681001dfa46 | 117 | |
bwang | 42:030e0ec4eac5 | 118 | /*output to timers*/ |
bwang | 47:1c9868e226d0 | 119 | set_dtc(io.a, 0.5f + 0.5f * va); |
bwang | 47:1c9868e226d0 | 120 | set_dtc(io.b, 0.5f + 0.5f * vb); |
bwang | 47:1c9868e226d0 | 121 | set_dtc(io.c, 0.5f + 0.5f * vc); |
bwang | 42:030e0ec4eac5 | 122 | } |
bwang | 42:030e0ec4eac5 | 123 | |
bwang | 44:3fd6a43b91f0 | 124 | void slow_loop() { |
bwang | 70:5e39beeb4a21 | 125 | control.torque_percent = update_filter(control.torque_percent, th->map(io.throttle_in->get_throttle(), read.w), THROTTLE_FILTER_STRENGTH); |
bwang | 70:5e39beeb4a21 | 126 | dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref); |
bwang | 44:3fd6a43b91f0 | 127 | } |
bwang | 44:3fd6a43b91f0 | 128 | |
bwang | 42:030e0ec4eac5 | 129 | void go_enabled() { |
bwang | 42:030e0ec4eac5 | 130 | control_enabled = true; |
bwang | 42:030e0ec4eac5 | 131 | io.en->write(1); |
bwang | 42:030e0ec4eac5 | 132 | } |
bwang | 42:030e0ec4eac5 | 133 | |
bwang | 42:030e0ec4eac5 | 134 | void go_disabled() { |
bwang | 52:fd3d8df99287 | 135 | control.d_integral = 0.0f; |
bwang | 52:fd3d8df99287 | 136 | control.q_integral = 0.0f; |
bwang | 42:030e0ec4eac5 | 137 | control_enabled = false; |
bwang | 42:030e0ec4eac5 | 138 | io.en->write(0); |
bwang | 42:030e0ec4eac5 | 139 | } |
bwang | 42:030e0ec4eac5 | 140 | |
bwang | 52:fd3d8df99287 | 141 | bool is_driving() { |
bwang | 55:3568b78dc203 | 142 | return io.throttle_in->get_throttle() > 0.01f || fabsf(read.w) > W_SAFE; |
bwang | 52:fd3d8df99287 | 143 | } |
bwang | 52:fd3d8df99287 | 144 | |
bwang | 44:3fd6a43b91f0 | 145 | float update_filter(float old, float x, float str) { |
bwang | 42:030e0ec4eac5 | 146 | return str * old + (1.0f - str) * x; |
bwang | 42:030e0ec4eac5 | 147 | } |
dicarloj | 13:41d102a53caf | 148 | |
bwang | 70:5e39beeb4a21 | 149 | void debug_print() { |
bwang | 70:5e39beeb4a21 | 150 | 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 | 70:5e39beeb4a21 | 151 | (int) (255 * control.d_integral), (int) (255 * control.q_integral)); |
bwang | 71:292dfc44ca53 | 152 | //io.pc->printf("%d\n", (int) (255 * control.torque_percent)); |
bwang | 70:5e39beeb4a21 | 153 | wait(0.01); |
bwang | 70:5e39beeb4a21 | 154 | } |
bwang | 70:5e39beeb4a21 | 155 | |
bwang | 70:5e39beeb4a21 | 156 | |
bwang | 1:7b61790f6be9 | 157 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) { |
bwang | 1:7b61790f6be9 | 158 | if (TIM1->SR & TIM_SR_UIF ) { |
bwang | 4:a6669248ce4d | 159 | ADC1->CR2 |= 0x40000000; |
bwang | 4:a6669248ce4d | 160 | volatile int delay; |
bwang | 4:a6669248ce4d | 161 | for (delay = 0; delay < 35; delay++); |
bwang | 42:030e0ec4eac5 | 162 | read.adval1 = ADC1->DR; |
bwang | 42:030e0ec4eac5 | 163 | read.adval2 = ADC2->DR; |
bwang | 4:a6669248ce4d | 164 | commutate(); |
bwang | 1:7b61790f6be9 | 165 | } |
bwang | 1:7b61790f6be9 | 166 | TIM1->SR = 0x00; |
bwang | 1:7b61790f6be9 | 167 | } |
bwang | 1:7b61790f6be9 | 168 | |
bwang | 0:bac9c3a3a6ca | 169 | int main() { |
bwang | 59:0416da6c3060 | 170 | dq = new LutMapper(); |
bwang | 45:cf8ad81fb0f0 | 171 | th = new NullThrottleMapper(); |
bwang | 47:1c9868e226d0 | 172 | BREMSInit(&io, &read, &foc, &control, false); |
bwang | 48:a1a09c83d42c | 173 | |
bwang | 0:bac9c3a3a6ca | 174 | for (;;) { |
bwang | 70:5e39beeb4a21 | 175 | debug_print(); |
bwang | 0:bac9c3a3a6ca | 176 | } |
bwang | 42:030e0ec4eac5 | 177 | } |