LAB4_dc_motor
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #define reduction_ratio 29 00003 00004 // hall sensor 00005 InterruptIn motor1_hallA(A1); 00006 InterruptIn motor1_hallB(A2); 00007 InterruptIn motor2_hallA(D13); 00008 InterruptIn motor2_hallB(D12); 00009 00010 // decoder of hall sensor 00011 int motor1_state_now; 00012 int motor1_state_past; 00013 int motor2_state_now; 00014 int motor2_state_past; 00015 00016 // position of motor 00017 int motor1_pos = 0; 00018 int motor2_pos = 0; 00019 00020 // velocity 00021 double motor1_vel = 0; 00022 double motor2_vel = 0; 00023 00024 // functions 00025 void update_state(int motor); // trigged when hall changed 00026 int direction(int motor); // if forward, return 1 00027 00028 int main() 00029 { 00030 // detect if hall snesor signal changed 00031 motor1_hallA.rise(&update_state(1)); 00032 motor1_hallA.fall(&update_state(1)); 00033 motor1_hallB.rise(&update_state(1)); 00034 motor1_hallB.fall(&update_state(1)); 00035 00036 motor2_hallA.rise(&update_state(2)); 00037 motor2_hallA.fall(&update_state(2)); 00038 motor2_hallB.rise(&update_state(2)); 00039 motor2_hallB.fall(&update_state(2)); 00040 00041 } 00042 00043 void update_state(int motor) 00044 { 00045 /* 00046 hall | A B | int 00047 state1 | 0 0 | 1 00048 state2 | 0 1 | 2 00049 state3 | 1 1 | 3 00050 state4 | 1 0 | 4 00051 00052 */ 00053 int state_new; 00054 int state_temp; 00055 00056 // determine new state 00057 if(motor1_hallA.read()==0) 00058 { 00059 if(motor1_hallB.read()==0) 00060 { 00061 state_new = 1; 00062 } 00063 else //motor1_hallB.read()==1 00064 { 00065 state_new = 2; 00066 } 00067 } 00068 else // motor1_hallA.read()==1 00069 { 00070 if(motor1_hallB.read()==0) 00071 { 00072 state_new = 3; 00073 } 00074 else //motor1_hallB.read()==1 00075 { 00076 state_new = 4; 00077 } 00078 } 00079 // end of determine new state 00080 00081 // update state and exchange now to past 00082 if(motor==1) 00083 { 00084 state_temp = motor1_state_now; 00085 motor1_state_now = state_new; 00086 motor1_state_past = state_temp; 00087 } 00088 else // motor==2 00089 { 00090 state_temp = motor2_state_now; 00091 motor2_state_now = state_new; 00092 motor2_state_past = state_temp; 00093 } 00094 00095 // after changing the new state, determine the direction 00096 direction(motor); 00097 00098 } 00099 // end of update_state 00100 00101 00102 00103 int direction(int motor) 00104 { 00105 /* 00106 state 00107 1->2->3->4->1 forward 00108 4->3->2->1->4 backward 00109 */ 00110 00111 int state_change; 00112 00113 if(motor==1) 00114 { 00115 state_change = motor1_state_now - motor1_state_past; 00116 if(state_change==1 or state_change==-3) 00117 { 00118 // forward 00119 motor1_pos = motor1_pos + 1; 00120 return 1; 00121 } 00122 else if(state_change==-1 or state_change==3) 00123 { 00124 // backward 00125 motor1_pos = motor1_pos - 1; 00126 return 0; 00127 } 00128 } 00129 else // motor==2 00130 { 00131 state_change = motor2_state_now - motor2_state_past; 00132 if(state_change==1 or state_change==-3) 00133 { 00134 // forward 00135 motor2_pos = motor2_pos + 1; 00136 return 1; 00137 } 00138 else if(state_change==-1 or state_change==3) 00139 { 00140 // backward 00141 motor2_pos = motor2_pos - 1; 00142 return 0; 00143 } 00144 } 00145 } 00146 // end of direction
Generated on Mon Jul 18 2022 14:49:09 by
1.7.2