LAB4_dc_motor
Dependencies: mbed
Fork of DC_Motor by
Diff: main.cpp
- Revision:
- 0:8a0e6c62ba24
- Child:
- 1:687eb9cab6eb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Apr 01 17:03:07 2018 +0000 @@ -0,0 +1,146 @@ +#include "mbed.h" +#define reduction_ratio 29 + +// hall sensor +InterruptIn motor1_hallA(A1); +InterruptIn motor1_hallB(A2); +InterruptIn motor2_hallA(D13); +InterruptIn motor2_hallB(D12); + +// decoder of hall sensor +int motor1_state_now; +int motor1_state_past; +int motor2_state_now; +int motor2_state_past; + +// position of motor +int motor1_pos = 0; +int motor2_pos = 0; + +// velocity +double motor1_vel = 0; +double motor2_vel = 0; + +// functions +void update_state(int motor); // trigged when hall changed +int direction(int motor); // if forward, return 1 + +int main() +{ + // detect if hall snesor signal changed + motor1_hallA.rise(&update_state(1)); + motor1_hallA.fall(&update_state(1)); + motor1_hallB.rise(&update_state(1)); + motor1_hallB.fall(&update_state(1)); + + motor2_hallA.rise(&update_state(2)); + motor2_hallA.fall(&update_state(2)); + motor2_hallB.rise(&update_state(2)); + motor2_hallB.fall(&update_state(2)); + +} + +void update_state(int motor) +{ + /* + hall | A B | int + state1 | 0 0 | 1 + state2 | 0 1 | 2 + state3 | 1 1 | 3 + state4 | 1 0 | 4 + + */ + int state_new; + int state_temp; + + // determine new state + if(motor1_hallA.read()==0) + { + if(motor1_hallB.read()==0) + { + state_new = 1; + } + else //motor1_hallB.read()==1 + { + state_new = 2; + } + } + else // motor1_hallA.read()==1 + { + if(motor1_hallB.read()==0) + { + state_new = 3; + } + else //motor1_hallB.read()==1 + { + state_new = 4; + } + } + // end of determine new state + + // update state and exchange now to past + if(motor==1) + { + state_temp = motor1_state_now; + motor1_state_now = state_new; + motor1_state_past = state_temp; + } + else // motor==2 + { + state_temp = motor2_state_now; + motor2_state_now = state_new; + motor2_state_past = state_temp; + } + + // after changing the new state, determine the direction + direction(motor); + +} +// end of update_state + + + +int direction(int motor) +{ + /* + state + 1->2->3->4->1 forward + 4->3->2->1->4 backward + */ + + int state_change; + + if(motor==1) + { + state_change = motor1_state_now - motor1_state_past; + if(state_change==1 or state_change==-3) + { + // forward + motor1_pos = motor1_pos + 1; + return 1; + } + else if(state_change==-1 or state_change==3) + { + // backward + motor1_pos = motor1_pos - 1; + return 0; + } + } + else // motor==2 + { + state_change = motor2_state_now - motor2_state_past; + if(state_change==1 or state_change==-3) + { + // forward + motor2_pos = motor2_pos + 1; + return 1; + } + else if(state_change==-1 or state_change==3) + { + // backward + motor2_pos = motor2_pos - 1; + return 0; + } + } +} +// end of direction \ No newline at end of file