Diff: Movement.cpp

Revision:

1:401bcbb58715

Parent:

0:0285394cacfa

Child:

2:878b6fd215d7

--- a/Movement.cpp	Fri Mar 06 16:55:04 2020 +0000
+++ b/Movement.cpp	Tue Mar 17 13:13:52 2020 +0000
@@ -3,14 +3,52 @@
 #include "UltraSonic.h"
 #include "clickers.h"
 
+extern bool rStopped, fStopped;
+extern Motor Wheel;
+extern int HallState;
+extern void dist();
+extern Serial pc;
+int i = 0;
 void Direction (){
-    while(fStopped == false){
-         Wheel.Speed(1.0,1.0);//forward 100%
-         FwdStop();
-    }
-    
-    while(fStopped == true && rStopped == false){
-         Wheel.Speed(-1.0,-1.0);//forward 100%
-         RevStop();
-    }
+    switch(HallState)
+            {
+            case 0:
+                if(fStopped == false){
+                    dist();
+                    if (i<1)
+                    {
+                        Wheel.Speed(1.0,1.0);//forward 100%
+                        pc.printf("Do this once\n\r");
+                        i++;
+                    }
+                    
+                    //FwdStop();
+                }
+                else if (fStopped == true)
+                    HallState = 1;
+                break;
+            case 1:
+                if(fStopped == true && rStopped == false){
+                    Wheel.Speed(-1.0,-1.0);//backward 100%
+                    RevStop();
+                    dist();
+                }
+                else if(fStopped == true && rStopped == true)
+                    HallState = 2;
+                break;
+            case 2:
+                Wheel.Speed(1.0,-1.0);//Right 100%
+                wait_ms(500);
+                Wheel.Speed(1.0,1.0);//forward 100%
+                wait_ms(500);
+                Wheel.Speed(-1.0,1.0);//Left 100%
+                wait_ms(500);
+                Wheel.Stop();
+                HallState = 3;
+                break;
+            case 3:
+            {
+                
+            }
+            }    
 }
\ No newline at end of file