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