Ming Cheng
/
DC_Motor
LAB4_dc_motor
main.cpp@0:8a0e6c62ba24, 2018-04-01 (annotated)
- Committer:
- m56542321
- Date:
- Sun Apr 01 17:03:07 2018 +0000
- Revision:
- 0:8a0e6c62ba24
LAB4_dc_motor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m56542321 | 0:8a0e6c62ba24 | 1 | #include "mbed.h" |
m56542321 | 0:8a0e6c62ba24 | 2 | #define reduction_ratio 29 |
m56542321 | 0:8a0e6c62ba24 | 3 | |
m56542321 | 0:8a0e6c62ba24 | 4 | // hall sensor |
m56542321 | 0:8a0e6c62ba24 | 5 | InterruptIn motor1_hallA(A1); |
m56542321 | 0:8a0e6c62ba24 | 6 | InterruptIn motor1_hallB(A2); |
m56542321 | 0:8a0e6c62ba24 | 7 | InterruptIn motor2_hallA(D13); |
m56542321 | 0:8a0e6c62ba24 | 8 | InterruptIn motor2_hallB(D12); |
m56542321 | 0:8a0e6c62ba24 | 9 | |
m56542321 | 0:8a0e6c62ba24 | 10 | // decoder of hall sensor |
m56542321 | 0:8a0e6c62ba24 | 11 | int motor1_state_now; |
m56542321 | 0:8a0e6c62ba24 | 12 | int motor1_state_past; |
m56542321 | 0:8a0e6c62ba24 | 13 | int motor2_state_now; |
m56542321 | 0:8a0e6c62ba24 | 14 | int motor2_state_past; |
m56542321 | 0:8a0e6c62ba24 | 15 | |
m56542321 | 0:8a0e6c62ba24 | 16 | // position of motor |
m56542321 | 0:8a0e6c62ba24 | 17 | int motor1_pos = 0; |
m56542321 | 0:8a0e6c62ba24 | 18 | int motor2_pos = 0; |
m56542321 | 0:8a0e6c62ba24 | 19 | |
m56542321 | 0:8a0e6c62ba24 | 20 | // velocity |
m56542321 | 0:8a0e6c62ba24 | 21 | double motor1_vel = 0; |
m56542321 | 0:8a0e6c62ba24 | 22 | double motor2_vel = 0; |
m56542321 | 0:8a0e6c62ba24 | 23 | |
m56542321 | 0:8a0e6c62ba24 | 24 | // functions |
m56542321 | 0:8a0e6c62ba24 | 25 | void update_state(int motor); // trigged when hall changed |
m56542321 | 0:8a0e6c62ba24 | 26 | int direction(int motor); // if forward, return 1 |
m56542321 | 0:8a0e6c62ba24 | 27 | |
m56542321 | 0:8a0e6c62ba24 | 28 | int main() |
m56542321 | 0:8a0e6c62ba24 | 29 | { |
m56542321 | 0:8a0e6c62ba24 | 30 | // detect if hall snesor signal changed |
m56542321 | 0:8a0e6c62ba24 | 31 | motor1_hallA.rise(&update_state(1)); |
m56542321 | 0:8a0e6c62ba24 | 32 | motor1_hallA.fall(&update_state(1)); |
m56542321 | 0:8a0e6c62ba24 | 33 | motor1_hallB.rise(&update_state(1)); |
m56542321 | 0:8a0e6c62ba24 | 34 | motor1_hallB.fall(&update_state(1)); |
m56542321 | 0:8a0e6c62ba24 | 35 | |
m56542321 | 0:8a0e6c62ba24 | 36 | motor2_hallA.rise(&update_state(2)); |
m56542321 | 0:8a0e6c62ba24 | 37 | motor2_hallA.fall(&update_state(2)); |
m56542321 | 0:8a0e6c62ba24 | 38 | motor2_hallB.rise(&update_state(2)); |
m56542321 | 0:8a0e6c62ba24 | 39 | motor2_hallB.fall(&update_state(2)); |
m56542321 | 0:8a0e6c62ba24 | 40 | |
m56542321 | 0:8a0e6c62ba24 | 41 | } |
m56542321 | 0:8a0e6c62ba24 | 42 | |
m56542321 | 0:8a0e6c62ba24 | 43 | void update_state(int motor) |
m56542321 | 0:8a0e6c62ba24 | 44 | { |
m56542321 | 0:8a0e6c62ba24 | 45 | /* |
m56542321 | 0:8a0e6c62ba24 | 46 | hall | A B | int |
m56542321 | 0:8a0e6c62ba24 | 47 | state1 | 0 0 | 1 |
m56542321 | 0:8a0e6c62ba24 | 48 | state2 | 0 1 | 2 |
m56542321 | 0:8a0e6c62ba24 | 49 | state3 | 1 1 | 3 |
m56542321 | 0:8a0e6c62ba24 | 50 | state4 | 1 0 | 4 |
m56542321 | 0:8a0e6c62ba24 | 51 | |
m56542321 | 0:8a0e6c62ba24 | 52 | */ |
m56542321 | 0:8a0e6c62ba24 | 53 | int state_new; |
m56542321 | 0:8a0e6c62ba24 | 54 | int state_temp; |
m56542321 | 0:8a0e6c62ba24 | 55 | |
m56542321 | 0:8a0e6c62ba24 | 56 | // determine new state |
m56542321 | 0:8a0e6c62ba24 | 57 | if(motor1_hallA.read()==0) |
m56542321 | 0:8a0e6c62ba24 | 58 | { |
m56542321 | 0:8a0e6c62ba24 | 59 | if(motor1_hallB.read()==0) |
m56542321 | 0:8a0e6c62ba24 | 60 | { |
m56542321 | 0:8a0e6c62ba24 | 61 | state_new = 1; |
m56542321 | 0:8a0e6c62ba24 | 62 | } |
m56542321 | 0:8a0e6c62ba24 | 63 | else //motor1_hallB.read()==1 |
m56542321 | 0:8a0e6c62ba24 | 64 | { |
m56542321 | 0:8a0e6c62ba24 | 65 | state_new = 2; |
m56542321 | 0:8a0e6c62ba24 | 66 | } |
m56542321 | 0:8a0e6c62ba24 | 67 | } |
m56542321 | 0:8a0e6c62ba24 | 68 | else // motor1_hallA.read()==1 |
m56542321 | 0:8a0e6c62ba24 | 69 | { |
m56542321 | 0:8a0e6c62ba24 | 70 | if(motor1_hallB.read()==0) |
m56542321 | 0:8a0e6c62ba24 | 71 | { |
m56542321 | 0:8a0e6c62ba24 | 72 | state_new = 3; |
m56542321 | 0:8a0e6c62ba24 | 73 | } |
m56542321 | 0:8a0e6c62ba24 | 74 | else //motor1_hallB.read()==1 |
m56542321 | 0:8a0e6c62ba24 | 75 | { |
m56542321 | 0:8a0e6c62ba24 | 76 | state_new = 4; |
m56542321 | 0:8a0e6c62ba24 | 77 | } |
m56542321 | 0:8a0e6c62ba24 | 78 | } |
m56542321 | 0:8a0e6c62ba24 | 79 | // end of determine new state |
m56542321 | 0:8a0e6c62ba24 | 80 | |
m56542321 | 0:8a0e6c62ba24 | 81 | // update state and exchange now to past |
m56542321 | 0:8a0e6c62ba24 | 82 | if(motor==1) |
m56542321 | 0:8a0e6c62ba24 | 83 | { |
m56542321 | 0:8a0e6c62ba24 | 84 | state_temp = motor1_state_now; |
m56542321 | 0:8a0e6c62ba24 | 85 | motor1_state_now = state_new; |
m56542321 | 0:8a0e6c62ba24 | 86 | motor1_state_past = state_temp; |
m56542321 | 0:8a0e6c62ba24 | 87 | } |
m56542321 | 0:8a0e6c62ba24 | 88 | else // motor==2 |
m56542321 | 0:8a0e6c62ba24 | 89 | { |
m56542321 | 0:8a0e6c62ba24 | 90 | state_temp = motor2_state_now; |
m56542321 | 0:8a0e6c62ba24 | 91 | motor2_state_now = state_new; |
m56542321 | 0:8a0e6c62ba24 | 92 | motor2_state_past = state_temp; |
m56542321 | 0:8a0e6c62ba24 | 93 | } |
m56542321 | 0:8a0e6c62ba24 | 94 | |
m56542321 | 0:8a0e6c62ba24 | 95 | // after changing the new state, determine the direction |
m56542321 | 0:8a0e6c62ba24 | 96 | direction(motor); |
m56542321 | 0:8a0e6c62ba24 | 97 | |
m56542321 | 0:8a0e6c62ba24 | 98 | } |
m56542321 | 0:8a0e6c62ba24 | 99 | // end of update_state |
m56542321 | 0:8a0e6c62ba24 | 100 | |
m56542321 | 0:8a0e6c62ba24 | 101 | |
m56542321 | 0:8a0e6c62ba24 | 102 | |
m56542321 | 0:8a0e6c62ba24 | 103 | int direction(int motor) |
m56542321 | 0:8a0e6c62ba24 | 104 | { |
m56542321 | 0:8a0e6c62ba24 | 105 | /* |
m56542321 | 0:8a0e6c62ba24 | 106 | state |
m56542321 | 0:8a0e6c62ba24 | 107 | 1->2->3->4->1 forward |
m56542321 | 0:8a0e6c62ba24 | 108 | 4->3->2->1->4 backward |
m56542321 | 0:8a0e6c62ba24 | 109 | */ |
m56542321 | 0:8a0e6c62ba24 | 110 | |
m56542321 | 0:8a0e6c62ba24 | 111 | int state_change; |
m56542321 | 0:8a0e6c62ba24 | 112 | |
m56542321 | 0:8a0e6c62ba24 | 113 | if(motor==1) |
m56542321 | 0:8a0e6c62ba24 | 114 | { |
m56542321 | 0:8a0e6c62ba24 | 115 | state_change = motor1_state_now - motor1_state_past; |
m56542321 | 0:8a0e6c62ba24 | 116 | if(state_change==1 or state_change==-3) |
m56542321 | 0:8a0e6c62ba24 | 117 | { |
m56542321 | 0:8a0e6c62ba24 | 118 | // forward |
m56542321 | 0:8a0e6c62ba24 | 119 | motor1_pos = motor1_pos + 1; |
m56542321 | 0:8a0e6c62ba24 | 120 | return 1; |
m56542321 | 0:8a0e6c62ba24 | 121 | } |
m56542321 | 0:8a0e6c62ba24 | 122 | else if(state_change==-1 or state_change==3) |
m56542321 | 0:8a0e6c62ba24 | 123 | { |
m56542321 | 0:8a0e6c62ba24 | 124 | // backward |
m56542321 | 0:8a0e6c62ba24 | 125 | motor1_pos = motor1_pos - 1; |
m56542321 | 0:8a0e6c62ba24 | 126 | return 0; |
m56542321 | 0:8a0e6c62ba24 | 127 | } |
m56542321 | 0:8a0e6c62ba24 | 128 | } |
m56542321 | 0:8a0e6c62ba24 | 129 | else // motor==2 |
m56542321 | 0:8a0e6c62ba24 | 130 | { |
m56542321 | 0:8a0e6c62ba24 | 131 | state_change = motor2_state_now - motor2_state_past; |
m56542321 | 0:8a0e6c62ba24 | 132 | if(state_change==1 or state_change==-3) |
m56542321 | 0:8a0e6c62ba24 | 133 | { |
m56542321 | 0:8a0e6c62ba24 | 134 | // forward |
m56542321 | 0:8a0e6c62ba24 | 135 | motor2_pos = motor2_pos + 1; |
m56542321 | 0:8a0e6c62ba24 | 136 | return 1; |
m56542321 | 0:8a0e6c62ba24 | 137 | } |
m56542321 | 0:8a0e6c62ba24 | 138 | else if(state_change==-1 or state_change==3) |
m56542321 | 0:8a0e6c62ba24 | 139 | { |
m56542321 | 0:8a0e6c62ba24 | 140 | // backward |
m56542321 | 0:8a0e6c62ba24 | 141 | motor2_pos = motor2_pos - 1; |
m56542321 | 0:8a0e6c62ba24 | 142 | return 0; |
m56542321 | 0:8a0e6c62ba24 | 143 | } |
m56542321 | 0:8a0e6c62ba24 | 144 | } |
m56542321 | 0:8a0e6c62ba24 | 145 | } |
m56542321 | 0:8a0e6c62ba24 | 146 | // end of direction |