Diff: main.cpp

Revision:

26:57c62b925064

Parent:

25:321b970eb3ff

Child:

27:35c962e8e95b

--- a/main.cpp	Mon May 30 08:16:42 2016 +0000
+++ b/main.cpp	Mon May 30 09:15:51 2016 +0000
@@ -34,7 +34,7 @@
 DigitalOut IN12(D6);
 
 
-static int speed = 2000;
+static int speed = 5000;
 static int bootStep1 = 100;
 static int bootStep2 = 100;
 static int bootStep3 = 100;
@@ -44,6 +44,10 @@
 static bool e3 = true;
 static bool rest = false;
 
+static bool sync1 = false;
+static bool sync2 = false;
+static bool sync3 = false;
+
 static int steps1 = 0;
 static int steps2 = 0;
 static int steps3 = 0;
@@ -426,24 +430,28 @@
 }
 
 
-//MOTOR 1 LEFT SIDE BUTTON
+//MOTOR 1 RIGHT SIDE BUTTON
 void pb1_hit_callback (void) 
 {
     if(rest == false)
     {
         e1 = false;
         
-        speed = 2000;
-        Thread::wait(1000);
-        int stepsToCenter = 242;
-                    
-        while (stepsToCenter >= 0)
+        //speed = 2000;
+        if (sync1 == true)
         {
-            stepEngine1Right();
-            stepsToCenter --;
+            Thread::wait(1000);
+            
+            int stepsToCenter = steps1/2;
+                        
+            while (stepsToCenter >= 0)
+            {
+                stepEngine1Left();
+                stepsToCenter --;
+            }
+                //break;
+              
         }
-            //break;
-        //end if    
     }
     
     
@@ -451,7 +459,7 @@
 }
 
 
-//MOTOR 1 RIGHT SIDE BUTTON
+//MOTOR 1 LEFT SIDE BUTTON
 void pb2_hit_callback (void) 
 {
     if(rest == false)
@@ -459,14 +467,17 @@
         e1 = false;
         //m_cmd = 'x';
     
-        speed = 2000;
-        Thread::wait(1000);
-        int stepsToCenter = 242;
-                    
-        while (stepsToCenter >= 0)
+        //speed = 2000;
+        if (sync1 == true)
         {
-            stepEngine1Left();
-            stepsToCenter --;
+            Thread::wait(1000);
+            int stepsToCenter = 242;
+                        
+            while (stepsToCenter >= 0)
+            {
+                stepEngine1Right();
+                stepsToCenter --;
+            }
         }
     }
   
@@ -583,8 +594,21 @@
                 stepEngine1Right();   
             }
             stepEngine1Left();
+            //Found corner!
             if (e1 == false) 
             {
+                e1= true;
+                Thread::wait(1000);
+                while(true)
+                {
+                    steps1++;
+                    //Thread::wait(100);
+                    pc.printf("%d", steps1);
+                    stepEngine1Right();
+                    if(e1==false)
+                    break;
+                }
+                
                 Thread::wait(1000);
                 break; 
             }//end if
@@ -611,8 +635,6 @@
             stepEngine2Left();
             if (e2 == false) 
             {
-                pc.printf("HALLO");
-                
                 Thread::wait(1000);
                 break;
             }//end if