Senior design sensored code to run freescale motor with IM07M1; Ticker algorithm control
Fork of Blue_Board_Test_2 by
main.cpp@10:3012bacdfc55, 2016-04-13 (annotated)
- Committer:
- BVanderWilp
- Date:
- Wed Apr 13 16:24:11 2016 +0000
- Revision:
- 10:3012bacdfc55
- Parent:
- 9:af60c737a93e
- Child:
- 11:8dc1e36579e2
jumpStart now forces 3 steps instead of 2 for more consistent startup
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BVanderWilp | 0:d445abf9a8e9 | 1 | #include "mbed.h" |
BVanderWilp | 0:d445abf9a8e9 | 2 | |
BVanderWilp | 1:69c06d3676fd | 3 | PwmOut phaseA(PA_8); //Out1, Green |
BVanderWilp | 0:d445abf9a8e9 | 4 | DigitalOut phaseAEN(PC_10); |
BVanderWilp | 1:69c06d3676fd | 5 | PwmOut phaseB(PA_9); //Out2, Blue |
BVanderWilp | 0:d445abf9a8e9 | 6 | DigitalOut phaseBEN(PC_11); |
BVanderWilp | 1:69c06d3676fd | 7 | PwmOut phaseC(PA_10); //Out3, White |
BVanderWilp | 0:d445abf9a8e9 | 8 | DigitalOut phaseCEN(PC_12); |
BVanderWilp | 0:d445abf9a8e9 | 9 | |
BVanderWilp | 5:6110655353ad | 10 | AnalogIn pot(PB_1); |
BVanderWilp | 5:6110655353ad | 11 | |
BVanderWilp | 0:d445abf9a8e9 | 12 | InterruptIn button(USER_BUTTON); |
BVanderWilp | 0:d445abf9a8e9 | 13 | |
BVanderWilp | 0:d445abf9a8e9 | 14 | DigitalOut redLed(PB_2); |
BVanderWilp | 0:d445abf9a8e9 | 15 | DigitalOut led(LED1); |
BVanderWilp | 1:69c06d3676fd | 16 | InterruptIn hallA(PA_15); //H1, Green |
BVanderWilp | 1:69c06d3676fd | 17 | InterruptIn hallB(PB_3); //H2, Blue |
BVanderWilp | 1:69c06d3676fd | 18 | InterruptIn hallC(PB_10); //H3, White |
BVanderWilp | 0:d445abf9a8e9 | 19 | |
BVanderWilp | 7:9d90184335aa | 20 | //InterruptIn hallA(PB_3); //New: H2; Original: H1, Green |
BVanderWilp | 7:9d90184335aa | 21 | //InterruptIn hallB(PB_10); //New: H3; Original: H2, Blue |
BVanderWilp | 7:9d90184335aa | 22 | //InterruptIn hallC(PA_15); //New: H1; Original: H3, White |
BVanderWilp | 7:9d90184335aa | 23 | |
BVanderWilp | 6:e401c592d2c3 | 24 | Ticker rpmInterrupt; |
BVanderWilp | 6:e401c592d2c3 | 25 | int revCount = 0; |
BVanderWilp | 6:e401c592d2c3 | 26 | int rpmPrintFlag = 0; |
BVanderWilp | 6:e401c592d2c3 | 27 | int currentRPM; |
BVanderWilp | 0:d445abf9a8e9 | 28 | |
BVanderWilp | 5:6110655353ad | 29 | float pwmMax = 0.9; |
BVanderWilp | 5:6110655353ad | 30 | float pwmDuty; |
BVanderWilp | 0:d445abf9a8e9 | 31 | int stall = 0; |
BVanderWilp | 7:9d90184335aa | 32 | int reverse = 0; |
BVanderWilp | 6:e401c592d2c3 | 33 | |
BVanderWilp | 6:e401c592d2c3 | 34 | void rpmCalc() |
BVanderWilp | 6:e401c592d2c3 | 35 | { |
BVanderWilp | 7:9d90184335aa | 36 | currentRPM = revCount * 60; //account for elec vs mech rpm |
BVanderWilp | 6:e401c592d2c3 | 37 | revCount = 0; |
BVanderWilp | 6:e401c592d2c3 | 38 | rpmPrintFlag = 1; |
BVanderWilp | 6:e401c592d2c3 | 39 | } |
BVanderWilp | 9:af60c737a93e | 40 | //original names: CBA CBA, new names: BAC BAC |
BVanderWilp | 9:af60c737a93e | 41 | void Brise() //state1, A0 B- C+ |
BVanderWilp | 0:d445abf9a8e9 | 42 | { |
BVanderWilp | 0:d445abf9a8e9 | 43 | phaseAEN = 0; |
BVanderWilp | 1:69c06d3676fd | 44 | phaseBEN = 1; |
BVanderWilp | 1:69c06d3676fd | 45 | |
BVanderWilp | 3:2bcc36fe4de5 | 46 | phaseC.write(pwmDuty); |
BVanderWilp | 3:2bcc36fe4de5 | 47 | phaseCEN = 1; |
BVanderWilp | 7:9d90184335aa | 48 | // redLed = ! redLed; |
BVanderWilp | 6:e401c592d2c3 | 49 | revCount++; |
BVanderWilp | 1:69c06d3676fd | 50 | } |
BVanderWilp | 1:69c06d3676fd | 51 | |
BVanderWilp | 9:af60c737a93e | 52 | void Afall() //state2, A+ B- C0 |
BVanderWilp | 1:69c06d3676fd | 53 | { |
BVanderWilp | 1:69c06d3676fd | 54 | phaseCEN = 0; |
BVanderWilp | 1:69c06d3676fd | 55 | phaseC.write(0); |
BVanderWilp | 1:69c06d3676fd | 56 | phaseA.write(pwmDuty); |
BVanderWilp | 1:69c06d3676fd | 57 | phaseAEN = 1; |
BVanderWilp | 3:2bcc36fe4de5 | 58 | |
BVanderWilp | 3:2bcc36fe4de5 | 59 | phaseBEN = 1; |
BVanderWilp | 1:69c06d3676fd | 60 | } |
BVanderWilp | 1:69c06d3676fd | 61 | |
BVanderWilp | 9:af60c737a93e | 62 | void Crise() //state3, A+ B0 C- |
BVanderWilp | 1:69c06d3676fd | 63 | { |
BVanderWilp | 1:69c06d3676fd | 64 | phaseBEN = 0; |
BVanderWilp | 1:69c06d3676fd | 65 | phaseCEN = 1; |
BVanderWilp | 3:2bcc36fe4de5 | 66 | |
BVanderWilp | 3:2bcc36fe4de5 | 67 | phaseA.write(pwmDuty); |
BVanderWilp | 3:2bcc36fe4de5 | 68 | phaseAEN = 1; |
BVanderWilp | 1:69c06d3676fd | 69 | } |
BVanderWilp | 1:69c06d3676fd | 70 | |
BVanderWilp | 9:af60c737a93e | 71 | void Bfall() //state4, A0 B+ C- |
BVanderWilp | 1:69c06d3676fd | 72 | { |
BVanderWilp | 1:69c06d3676fd | 73 | phaseAEN = 0; |
BVanderWilp | 0:d445abf9a8e9 | 74 | phaseA.write(0); |
BVanderWilp | 0:d445abf9a8e9 | 75 | phaseB.write(pwmDuty); |
BVanderWilp | 0:d445abf9a8e9 | 76 | phaseBEN = 1; |
BVanderWilp | 1:69c06d3676fd | 77 | |
BVanderWilp | 3:2bcc36fe4de5 | 78 | phaseCEN = 1; |
BVanderWilp | 7:9d90184335aa | 79 | // redLed = ! redLed; |
BVanderWilp | 0:d445abf9a8e9 | 80 | } |
BVanderWilp | 0:d445abf9a8e9 | 81 | |
BVanderWilp | 9:af60c737a93e | 82 | void Arise() //state5, A- B+ C0 |
BVanderWilp | 0:d445abf9a8e9 | 83 | { |
BVanderWilp | 0:d445abf9a8e9 | 84 | phaseCEN = 0; |
BVanderWilp | 0:d445abf9a8e9 | 85 | phaseAEN = 1; |
BVanderWilp | 3:2bcc36fe4de5 | 86 | |
BVanderWilp | 3:2bcc36fe4de5 | 87 | phaseB.write(pwmDuty); |
BVanderWilp | 3:2bcc36fe4de5 | 88 | phaseBEN = 1; |
BVanderWilp | 0:d445abf9a8e9 | 89 | } |
BVanderWilp | 0:d445abf9a8e9 | 90 | |
BVanderWilp | 9:af60c737a93e | 91 | void Cfall() //state6, A- B0 C+ |
BVanderWilp | 0:d445abf9a8e9 | 92 | { |
BVanderWilp | 0:d445abf9a8e9 | 93 | phaseBEN = 0; |
BVanderWilp | 0:d445abf9a8e9 | 94 | phaseB.write(0); |
BVanderWilp | 0:d445abf9a8e9 | 95 | phaseC.write(pwmDuty); |
BVanderWilp | 0:d445abf9a8e9 | 96 | phaseCEN = 1; |
BVanderWilp | 3:2bcc36fe4de5 | 97 | |
BVanderWilp | 3:2bcc36fe4de5 | 98 | phaseAEN = 1; |
BVanderWilp | 0:d445abf9a8e9 | 99 | } |
BVanderWilp | 0:d445abf9a8e9 | 100 | |
BVanderWilp | 0:d445abf9a8e9 | 101 | |
BVanderWilp | 7:9d90184335aa | 102 | void jumpStart() |
BVanderWilp | 7:9d90184335aa | 103 | { |
BVanderWilp | 3:2bcc36fe4de5 | 104 | int h1 = hallA.read(); |
BVanderWilp | 3:2bcc36fe4de5 | 105 | int h2 = hallB.read(); |
BVanderWilp | 3:2bcc36fe4de5 | 106 | int h3 = hallC.read(); |
BVanderWilp | 3:2bcc36fe4de5 | 107 | //check where we start |
BVanderWilp | 3:2bcc36fe4de5 | 108 | if(h1 == 0 && h2 == 1 && h3 == 1) //state1 |
BVanderWilp | 3:2bcc36fe4de5 | 109 | { |
BVanderWilp | 9:af60c737a93e | 110 | Brise(); |
BVanderWilp | 9:af60c737a93e | 111 | Afall(); |
BVanderWilp | 10:3012bacdfc55 | 112 | Crise(); |
BVanderWilp | 9:af60c737a93e | 113 | } |
BVanderWilp | 9:af60c737a93e | 114 | else if(h1 == 0 && h2 == 0 && h3 == 1) //state2 |
BVanderWilp | 9:af60c737a93e | 115 | { |
BVanderWilp | 9:af60c737a93e | 116 | Afall(); |
BVanderWilp | 9:af60c737a93e | 117 | Crise(); |
BVanderWilp | 10:3012bacdfc55 | 118 | Bfall(); |
BVanderWilp | 9:af60c737a93e | 119 | } |
BVanderWilp | 9:af60c737a93e | 120 | else if(h1 == 1 && h2 == 0 && h3 == 1) //state3 |
BVanderWilp | 9:af60c737a93e | 121 | { |
BVanderWilp | 3:2bcc36fe4de5 | 122 | Crise(); |
BVanderWilp | 3:2bcc36fe4de5 | 123 | Bfall(); |
BVanderWilp | 10:3012bacdfc55 | 124 | Arise(); |
BVanderWilp | 3:2bcc36fe4de5 | 125 | } |
BVanderWilp | 9:af60c737a93e | 126 | else if(h1 == 1 && h2 == 0 && h3 == 0) //state4 |
BVanderWilp | 3:2bcc36fe4de5 | 127 | { |
BVanderWilp | 3:2bcc36fe4de5 | 128 | Bfall(); |
BVanderWilp | 3:2bcc36fe4de5 | 129 | Arise(); |
BVanderWilp | 10:3012bacdfc55 | 130 | Cfall(); |
BVanderWilp | 3:2bcc36fe4de5 | 131 | } |
BVanderWilp | 9:af60c737a93e | 132 | else if(h1 == 1 && h2 == 1 && h3 == 0) //state5 |
BVanderWilp | 3:2bcc36fe4de5 | 133 | { |
BVanderWilp | 3:2bcc36fe4de5 | 134 | Arise(); |
BVanderWilp | 3:2bcc36fe4de5 | 135 | Cfall(); |
BVanderWilp | 10:3012bacdfc55 | 136 | Brise(); |
BVanderWilp | 3:2bcc36fe4de5 | 137 | } |
BVanderWilp | 9:af60c737a93e | 138 | else //(h1 == 0 && h2 == 1 && h3 == 0)state6 |
BVanderWilp | 3:2bcc36fe4de5 | 139 | { |
BVanderWilp | 3:2bcc36fe4de5 | 140 | Cfall(); |
BVanderWilp | 3:2bcc36fe4de5 | 141 | Brise(); |
BVanderWilp | 10:3012bacdfc55 | 142 | Afall(); |
BVanderWilp | 3:2bcc36fe4de5 | 143 | } |
BVanderWilp | 7:9d90184335aa | 144 | } |
BVanderWilp | 7:9d90184335aa | 145 | |
BVanderWilp | 7:9d90184335aa | 146 | void activate() |
BVanderWilp | 7:9d90184335aa | 147 | { |
BVanderWilp | 7:9d90184335aa | 148 | if(stall == 0) |
BVanderWilp | 7:9d90184335aa | 149 | { |
BVanderWilp | 7:9d90184335aa | 150 | stall = 1; |
BVanderWilp | 7:9d90184335aa | 151 | } |
BVanderWilp | 7:9d90184335aa | 152 | else |
BVanderWilp | 7:9d90184335aa | 153 | { |
BVanderWilp | 7:9d90184335aa | 154 | jumpStart(); |
BVanderWilp | 7:9d90184335aa | 155 | // if(reverse == 1) //switch to forward |
BVanderWilp | 7:9d90184335aa | 156 | // { |
BVanderWilp | 7:9d90184335aa | 157 | // hallA.fall(&Afall); |
BVanderWilp | 7:9d90184335aa | 158 | // hallA.rise(&Arise); |
BVanderWilp | 7:9d90184335aa | 159 | // hallB.fall(&Bfall); |
BVanderWilp | 7:9d90184335aa | 160 | // hallB.rise(&Brise); |
BVanderWilp | 7:9d90184335aa | 161 | // hallC.fall(&Cfall); |
BVanderWilp | 7:9d90184335aa | 162 | // hallC.rise(&Crise); |
BVanderWilp | 7:9d90184335aa | 163 | // redLed = !redLed; |
BVanderWilp | 7:9d90184335aa | 164 | // reverse = 0; |
BVanderWilp | 7:9d90184335aa | 165 | // } |
BVanderWilp | 7:9d90184335aa | 166 | // else if(reverse == 0) //switch to reverse |
BVanderWilp | 7:9d90184335aa | 167 | // { |
BVanderWilp | 7:9d90184335aa | 168 | // hallA.fall(&Bfall); |
BVanderWilp | 7:9d90184335aa | 169 | // hallA.rise(&Brise); |
BVanderWilp | 7:9d90184335aa | 170 | // hallB.fall(&Cfall); |
BVanderWilp | 7:9d90184335aa | 171 | // hallB.rise(&Crise); |
BVanderWilp | 7:9d90184335aa | 172 | // hallC.fall(&Afall); |
BVanderWilp | 7:9d90184335aa | 173 | // hallC.rise(&Arise); |
BVanderWilp | 7:9d90184335aa | 174 | // redLed = !redLed; |
BVanderWilp | 7:9d90184335aa | 175 | // reverse = 1; |
BVanderWilp | 7:9d90184335aa | 176 | // } |
BVanderWilp | 7:9d90184335aa | 177 | } |
BVanderWilp | 7:9d90184335aa | 178 | } |
BVanderWilp | 7:9d90184335aa | 179 | |
BVanderWilp | 7:9d90184335aa | 180 | int main() { |
BVanderWilp | 7:9d90184335aa | 181 | //wait until button push to start |
BVanderWilp | 7:9d90184335aa | 182 | rpmInterrupt.attach(&rpmCalc, 1); |
BVanderWilp | 7:9d90184335aa | 183 | button.rise(&activate); |
BVanderWilp | 7:9d90184335aa | 184 | while(stall == 0) { |
BVanderWilp | 7:9d90184335aa | 185 | led = !led; |
BVanderWilp | 7:9d90184335aa | 186 | wait(1); |
BVanderWilp | 7:9d90184335aa | 187 | } |
BVanderWilp | 7:9d90184335aa | 188 | |
BVanderWilp | 7:9d90184335aa | 189 | pwmDuty = pot.read() * pwmMax; |
BVanderWilp | 7:9d90184335aa | 190 | |
BVanderWilp | 7:9d90184335aa | 191 | phaseA.period_us(50); |
BVanderWilp | 7:9d90184335aa | 192 | phaseB.period_us(50); |
BVanderWilp | 7:9d90184335aa | 193 | phaseC.period_us(50); |
BVanderWilp | 7:9d90184335aa | 194 | |
BVanderWilp | 7:9d90184335aa | 195 | phaseA.write(0); |
BVanderWilp | 7:9d90184335aa | 196 | phaseB.write(0); |
BVanderWilp | 7:9d90184335aa | 197 | phaseC.write(0); |
BVanderWilp | 7:9d90184335aa | 198 | |
BVanderWilp | 7:9d90184335aa | 199 | phaseAEN = 0; |
BVanderWilp | 7:9d90184335aa | 200 | phaseBEN = 0; |
BVanderWilp | 7:9d90184335aa | 201 | phaseCEN = 0; |
BVanderWilp | 7:9d90184335aa | 202 | //begin sensored mode |
BVanderWilp | 7:9d90184335aa | 203 | hallA.fall(&Afall); |
BVanderWilp | 7:9d90184335aa | 204 | hallA.rise(&Arise); |
BVanderWilp | 7:9d90184335aa | 205 | hallB.fall(&Bfall); |
BVanderWilp | 7:9d90184335aa | 206 | hallB.rise(&Brise); |
BVanderWilp | 7:9d90184335aa | 207 | hallC.fall(&Cfall); |
BVanderWilp | 7:9d90184335aa | 208 | hallC.rise(&Crise); |
BVanderWilp | 7:9d90184335aa | 209 | |
BVanderWilp | 7:9d90184335aa | 210 | jumpStart(); |
BVanderWilp | 5:6110655353ad | 211 | float ADCSum = 0;; |
BVanderWilp | 5:6110655353ad | 212 | int ADCCount = 0; |
BVanderWilp | 0:d445abf9a8e9 | 213 | while(1) { |
BVanderWilp | 0:d445abf9a8e9 | 214 | led = !led; |
BVanderWilp | 5:6110655353ad | 215 | ADCSum += pot.read(); |
BVanderWilp | 5:6110655353ad | 216 | ADCCount++; |
BVanderWilp | 5:6110655353ad | 217 | if(ADCCount == 20) |
BVanderWilp | 5:6110655353ad | 218 | { |
BVanderWilp | 5:6110655353ad | 219 | pwmDuty = (ADCSum/20) * pwmMax; |
BVanderWilp | 5:6110655353ad | 220 | ADCSum = 0; |
BVanderWilp | 5:6110655353ad | 221 | ADCCount = 0; |
BVanderWilp | 5:6110655353ad | 222 | } |
BVanderWilp | 6:e401c592d2c3 | 223 | if(rpmPrintFlag == 1) |
BVanderWilp | 6:e401c592d2c3 | 224 | { |
BVanderWilp | 7:9d90184335aa | 225 | printf("%d rpm; %f duty\r\n", currentRPM, pwmDuty); |
BVanderWilp | 6:e401c592d2c3 | 226 | rpmPrintFlag = 0; |
BVanderWilp | 6:e401c592d2c3 | 227 | } |
BVanderWilp | 5:6110655353ad | 228 | wait(0.05); |
BVanderWilp | 0:d445abf9a8e9 | 229 | } |
BVanderWilp | 0:d445abf9a8e9 | 230 | } |