LAB4_dc_motor

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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