Roger Weng
/
MotorControl
Motor control on mbed
main.cpp@1:f47edcd44466, 2016-07-23 (annotated)
- Committer:
- roger5641
- Date:
- Sat Jul 23 16:00:05 2016 +0000
- Revision:
- 1:f47edcd44466
- Parent:
- 0:f76be6916639
CN interrupt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
roger5641 | 0:f76be6916639 | 1 | #include "mbed.h" |
roger5641 | 0:f76be6916639 | 2 | |
roger5641 | 0:f76be6916639 | 3 | //Motor sensor |
roger5641 | 0:f76be6916639 | 4 | InterruptIn HallA(D3); |
roger5641 | 0:f76be6916639 | 5 | InterruptIn HallB(D4); |
roger5641 | 0:f76be6916639 | 6 | InterruptIn HallC(D5); |
roger5641 | 0:f76be6916639 | 7 | |
roger5641 | 0:f76be6916639 | 8 | void init_CN(void); |
roger5641 | 0:f76be6916639 | 9 | void CN_interrupt(void); |
roger5641 | 0:f76be6916639 | 10 | |
roger5641 | 1:f47edcd44466 | 11 | int8_t stateA=0, stateB=0, stateC=0; |
roger5641 | 0:f76be6916639 | 12 | int8_t state_1 = 0, state_1_old = 0; |
roger5641 | 1:f47edcd44466 | 13 | int vCount; |
roger5641 | 0:f76be6916639 | 14 | |
roger5641 | 0:f76be6916639 | 15 | int main() { |
roger5641 | 1:f47edcd44466 | 16 | init_CN(); |
roger5641 | 0:f76be6916639 | 17 | |
roger5641 | 0:f76be6916639 | 18 | while(1) |
roger5641 | 0:f76be6916639 | 19 | { |
roger5641 | 0:f76be6916639 | 20 | ; |
roger5641 | 0:f76be6916639 | 21 | } |
roger5641 | 0:f76be6916639 | 22 | } |
roger5641 | 0:f76be6916639 | 23 | |
roger5641 | 1:f47edcd44466 | 24 | void CN_interrupt(void) |
roger5641 | 1:f47edcd44466 | 25 | { |
roger5641 | 1:f47edcd44466 | 26 | //Motor 1 |
roger5641 | 1:f47edcd44466 | 27 | stateA = HallA.read(); |
roger5641 | 1:f47edcd44466 | 28 | stateB = HallB.read(); |
roger5641 | 1:f47edcd44466 | 29 | stateC = HallC.read(); |
roger5641 | 1:f47edcd44466 | 30 | |
roger5641 | 1:f47edcd44466 | 31 | ///code for state determination/// |
roger5641 | 1:f47edcd44466 | 32 | if (stateA == 1) |
roger5641 | 1:f47edcd44466 | 33 | { |
roger5641 | 1:f47edcd44466 | 34 | if (stateB == 1) |
roger5641 | 1:f47edcd44466 | 35 | if(stateC == 0) |
roger5641 | 1:f47edcd44466 | 36 | { |
roger5641 | 1:f47edcd44466 | 37 | state_1 = 2; |
roger5641 | 1:f47edcd44466 | 38 | } |
roger5641 | 1:f47edcd44466 | 39 | else |
roger5641 | 1:f47edcd44466 | 40 | { |
roger5641 | 1:f47edcd44466 | 41 | ; |
roger5641 | 1:f47edcd44466 | 42 | } |
roger5641 | 1:f47edcd44466 | 43 | else |
roger5641 | 1:f47edcd44466 | 44 | if(stateC == 1) |
roger5641 | 1:f47edcd44466 | 45 | { |
roger5641 | 1:f47edcd44466 | 46 | state_1 = 6; |
roger5641 | 1:f47edcd44466 | 47 | } |
roger5641 | 1:f47edcd44466 | 48 | else |
roger5641 | 1:f47edcd44466 | 49 | { |
roger5641 | 1:f47edcd44466 | 50 | state_1 = 1; |
roger5641 | 1:f47edcd44466 | 51 | } |
roger5641 | 1:f47edcd44466 | 52 | } |
roger5641 | 1:f47edcd44466 | 53 | else |
roger5641 | 1:f47edcd44466 | 54 | { |
roger5641 | 1:f47edcd44466 | 55 | if (stateB == 1) |
roger5641 | 1:f47edcd44466 | 56 | if(stateC == 1) |
roger5641 | 1:f47edcd44466 | 57 | { |
roger5641 | 1:f47edcd44466 | 58 | state_1 = 4; |
roger5641 | 1:f47edcd44466 | 59 | } |
roger5641 | 1:f47edcd44466 | 60 | else |
roger5641 | 1:f47edcd44466 | 61 | { |
roger5641 | 1:f47edcd44466 | 62 | state_1 = 3; |
roger5641 | 1:f47edcd44466 | 63 | } |
roger5641 | 1:f47edcd44466 | 64 | else |
roger5641 | 1:f47edcd44466 | 65 | if(stateC == 1) |
roger5641 | 1:f47edcd44466 | 66 | { |
roger5641 | 1:f47edcd44466 | 67 | state_1 = 5; |
roger5641 | 1:f47edcd44466 | 68 | } |
roger5641 | 1:f47edcd44466 | 69 | else |
roger5641 | 1:f47edcd44466 | 70 | { |
roger5641 | 1:f47edcd44466 | 71 | ; |
roger5641 | 1:f47edcd44466 | 72 | } |
roger5641 | 1:f47edcd44466 | 73 | } |
roger5641 | 1:f47edcd44466 | 74 | |
roger5641 | 1:f47edcd44466 | 75 | //Forward: vCount +1 |
roger5641 | 1:f47edcd44466 | 76 | //Inverse: vCount -1 |
roger5641 | 1:f47edcd44466 | 77 | if ( (state_1 == (state_1_old + 1)) || (state_1 == 1 && state_1_old == 6) ) |
roger5641 | 1:f47edcd44466 | 78 | vCount++; |
roger5641 | 1:f47edcd44466 | 79 | else if ( (state_1 == (state_1_old - 1)) || (state_1 == 6 && state_1_old == 1)) |
roger5641 | 1:f47edcd44466 | 80 | vCount--; |
roger5641 | 1:f47edcd44466 | 81 | |
roger5641 | 1:f47edcd44466 | 82 | state_1_old = state_1; |
roger5641 | 1:f47edcd44466 | 83 | } |
roger5641 | 0:f76be6916639 | 84 | |
roger5641 | 0:f76be6916639 | 85 | void init_CN(void) |
roger5641 | 0:f76be6916639 | 86 | { |
roger5641 | 0:f76be6916639 | 87 | HallA.rise(&CN_interrupt); |
roger5641 | 0:f76be6916639 | 88 | HallA.fall(&CN_interrupt); |
roger5641 | 0:f76be6916639 | 89 | HallB.rise(&CN_interrupt); |
roger5641 | 0:f76be6916639 | 90 | HallB.fall(&CN_interrupt); |
roger5641 | 0:f76be6916639 | 91 | HallC.rise(&CN_interrupt); |
roger5641 | 0:f76be6916639 | 92 | HallC.fall(&CN_interrupt); |
roger5641 | 0:f76be6916639 | 93 | |
roger5641 | 0:f76be6916639 | 94 | stateA = HallA.read(); |
roger5641 | 0:f76be6916639 | 95 | stateB = HallB.read(); |
roger5641 | 0:f76be6916639 | 96 | stateC = HallC.read(); |
roger5641 | 0:f76be6916639 | 97 | |
roger5641 | 0:f76be6916639 | 98 | } |