Robotic Team 1 / Mbed 2 deprecated DC_Motor

LAB4_dc_motor

Dependencies:   mbed

Fork of DC_Motor by Ming Cheng

main.cpp@1:687eb9cab6eb, 2018-04-01 (annotated)

Committer:
MechanicalThomas
Date:
Sun Apr 01 17:55:11 2018 +0000
Revision:
1:687eb9cab6eb
Parent:
0:8a0e6c62ba24 
HELLO

Who changed what in which revision?

UserRevisionLine numberNew 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 }
MechanicalThomas 1:687eb9cab6eb 146 // end of direction
MechanicalThomas 1:687eb9cab6eb 147 //HELLO