Diff: main.cpp

Revision:

27:35c962e8e95b

Parent:

26:57c62b925064

Child:

28:dac262b7ab32

--- a/main.cpp	Mon May 30 09:15:51 2016 +0000
+++ b/main.cpp	Wed Jun 01 17:40:54 2016 +0000
@@ -34,26 +34,67 @@
 DigitalOut IN12(D6);
 
 
-static int speed = 5000;
+static int speed = 1750;
 static int bootStep1 = 100;
 static int bootStep2 = 100;
 static int bootStep3 = 100;
 static char m_cmd = 'x';
+static char cmd;
 static bool e1 = true;
 static bool e2 = true;
 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;
 
 
 //Methods to set pins to control direction.
+
+void step2Left3Right()
+{
+        //engine 2
+    IN5=1;
+    IN6=0;
+    IN7=0;
+    IN8=1;
+            //engine 3
+    IN9=0;
+    IN10=1;
+    IN11=0;
+    IN12=1;
+
+    wait_us(speed);
+    IN5=1;
+    IN6=0;
+    IN7=1;
+    IN8=0;
+    IN9=0;
+    IN10=1;
+    IN11=1;
+    IN12=0;
+    wait_us(speed);
+    IN5=0;
+    IN6=1;
+    IN7=1;
+    IN8=0;
+    IN9=1;
+    IN10=0;
+    IN11=1;
+    IN12=0;
+    wait_us(speed);
+    IN5=0;
+    IN6=1;
+    IN7=0;
+    IN8=1;
+    IN9=1;
+    IN10=0;
+    IN11=0;
+    IN12=1;
+    wait_us(speed);
+}
+
 void stepAllRight()
 {
     //engine 1
@@ -339,31 +380,6 @@
     wait_us(speed);
 }
 
-
-void step1()
-{
-    //engine 1
-    IN1=1;
-    IN2=0;
-    IN3=0;
-    IN4=1;
-}
-
-void step2()
-{
-    //engine 1
-    IN1=0;
-    IN2=1;
-    IN3=0;
-    IN4=1;
-}
-
-void wait()
-{
-    wait_us(speed);
-    
-}
-
 //Method to release all engines.
 void stopAll()
 {
@@ -407,10 +423,21 @@
     
 }
 
-
-
-
-
+void hold()
+{
+    IN1=1;
+    IN2=0;
+    IN3=0;
+    IN4=1;
+    IN5=1;
+    IN6=0;
+    IN7=0;
+    IN8=1;
+    IN9=1;
+    IN10=0;
+    IN11=0;
+    IN12=1;
+}
 
 
 void input(void const *args)
@@ -419,47 +446,55 @@
     {
 
         if(pc.readable()) 
-        { m_cmd=pc.getc(); 
-        
-        }
-         
+        { 
+            m_cmd=pc.getc(); 
+        }        
         Thread::wait(100);
         //pc.printf("%d", steps);
+       /* 
+         if (m_cmd == 'x')
+         {    
+            while(speed < 7000)
+            {
+                //Thread::wait(1);
+                speed ++;
+            }
+            speed = 2000;
+            cmd = 'x';
+            break;
+        }
+        */
+        
+        
         
     }
 }
 
 
-//MOTOR 1 RIGHT SIDE BUTTON
+//MOTOR 1 LEFT SIDE BUTTON
 void pb1_hit_callback (void) 
 {
     if(rest == false)
     {
         e1 = false;
         
-        //speed = 2000;
-        if (sync1 == true)
+        speed = 2000;
+        Thread::wait(1000);
+        int stepsToCenter = 242;
+                    
+        while (stepsToCenter >= 0)
         {
-            Thread::wait(1000);
-            
-            int stepsToCenter = steps1/2;
-                        
-            while (stepsToCenter >= 0)
-            {
-                stepEngine1Left();
-                stepsToCenter --;
-            }
-                //break;
-              
+            stepEngine1Right();
+            stepsToCenter --;
         }
+        steps1 = 0;
+            //break;
+        //end if
+        m_cmd = 'x';  
     }
-    
-    
-    
 }
 
-
-//MOTOR 1 LEFT SIDE BUTTON
+//MOTOR 1 RIGHT SIDE BUTTON
 void pb2_hit_callback (void) 
 {
     if(rest == false)
@@ -467,20 +502,18 @@
         e1 = false;
         //m_cmd = 'x';
     
-        //speed = 2000;
-        if (sync1 == true)
+        speed = 2000;
+        Thread::wait(1000);
+        int stepsToCenter = 242;
+                    
+        while (stepsToCenter >= 0)
         {
-            Thread::wait(1000);
-            int stepsToCenter = 242;
-                        
-            while (stepsToCenter >= 0)
-            {
-                stepEngine1Right();
-                stepsToCenter --;
-            }
+            stepEngine1Left();
+            stepsToCenter --;
         }
+        steps1 = 0;
+        m_cmd = 'x';
     }
-  
 }
 //MOTOR 2 LEFT SIDE BUTTON
 void pb3_hit_callback (void) 
@@ -495,9 +528,11 @@
                     
         while (stepsToCenter >= 0)
         {
-            stepEngine2Left();
+            stepEngine2Right();
             stepsToCenter --;
         }
+        steps2 = 0;
+        m_cmd = 'x';
     }
 
 }
@@ -515,9 +550,11 @@
                     
         while (stepsToCenter >= 0)
         {
-            stepEngine2Right();
+            stepEngine2Left();
             stepsToCenter --;
         }
+        steps2 = 0;
+        m_cmd = 'x';
     }
 
 
@@ -530,15 +567,17 @@
     {
         e3 = false;
         //m_cmd = 'x';
-        speed = 2000;
+        speed = 2500;
         Thread::wait(1000);
-        int stepsToCenter = 242;
+        int stepsToCenter = 200;
                     
         while (stepsToCenter >= 0)
         {
             stepEngine3Left();
             stepsToCenter --;
         }
+        steps3 = 0;
+        m_cmd = 'x';
     }
 
 
@@ -546,24 +585,29 @@
 //MOTOR 3 RIGHT SIDE BUTTON
 void pb6_hit_callback (void) 
 {
+    
     if(rest == false)
     {
         e3 = false;
         //m_cmd = 'x';
         speed = 2000;
         Thread::wait(1000);
-        int stepsToCenter = 242;
+        int stepsToCenter = 280;
                     
         while (stepsToCenter >= 0)
         {
             stepEngine3Right();
             stepsToCenter --;
         }
+        //reset steps and stop engine
+        steps3 = 0;
+        m_cmd = 'x';
     }
 }
 
 int main()
 {
+        int slowDown;
 
         pb1.mode(PullUp);
         pb2.mode(PullUp);
@@ -594,21 +638,8 @@
                 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
@@ -635,6 +666,7 @@
             stepEngine2Left();
             if (e2 == false) 
             {
+                
                 Thread::wait(1000);
                 break;
             }//end if
@@ -646,7 +678,7 @@
         pb6.setSampleFrequency();
         while (true)
         {
-            speed = 2000;
+            //speed = 2000;
             while(bootStep3 >= 0)
             {
                 bootStep3--;
@@ -656,7 +688,7 @@
             stepEngine3Left();
             if (e3 == false) 
             {
-                pc.printf("HALLO");
+                //pc.printf("HALLO");
                 Thread::wait(1000);
                 break;
             }//end if
@@ -664,17 +696,93 @@
 
         while (true) 
         {   
-            speed = 1500;
-            
+            //speed = 1500;
 
+            switch (m_cmd)
+            {
+                case '7':
+                cmd = '7';
+                break;
+                
+                case '9':
+                cmd = '9';
+                break;
+                
+                case '4':
+                cmd= '4';
+                break;
+                
+                case '6':
+                cmd = '6';
+                break;
+                
+                case '1':
+                cmd = '1';
+                break;
+                
+                case '3':
+                cmd = '3';
+                break;
+                
+                case 'x':
+                cmd = 'x';
+                break;
+                
+                //release motors
+                case 'z':
+                
+                //step all motors to the middle
+                
+                while(steps1 > 0)
+                {
+                    stepEngine1Left();
+                    steps1--;
+                }
+                while(steps1 < 0)
+                {
+                    stepEngine1Right();
+                    steps1++;
+                }
+                
+                while(steps2 > 0)
+                {
+                    stepEngine2Left();
+                    steps2--;
+                }
+                while(steps2 < 0)
+                {
+                    stepEngine2Right();
+                    steps2++;
+                }
+                
+                while(steps3 > 0)
+                {
+                    stepEngine3Left();
+                    steps3--;
+                }
+                while(steps3 < 0)
+                {
+                    stepEngine3Right();
+                    steps3++;
+                }
+
+                cmd = 'z';
+
+
+                break;
+                
+            }
+            
             //Release all motors / Set all pins to 0
-            if (m_cmd == 'z')
+            if (cmd == 'z')
             {
                 stopAll();
             } 
             //Make all engines halt and hold still. 
-            if (m_cmd == 'x')
+            if (cmd == 'x')
             {
+                hold();
+
             }
             
             //Rest mode. Make the engines ignore pushbuttons.
@@ -684,52 +792,96 @@
                 rest = true;
                 //Step motors to rest position
                 
-                
             }
             */
   
             //Controlling each motor seperately.
             //ENGINE 3
-            if(m_cmd == '7' && steps3 > -242)
+            if(cmd == '7' && steps3 > -200)
             {
                 rest = false;
                 
                 steps3 --;
                 stepEngine3Left();
+                
             }
-            else if (m_cmd == '9' && steps3 < 242)
+            else if (cmd == '9' && steps3 < 280)
             { 
                 rest = false;
                 stepEngine3Right();
                 steps3 ++;
             }
             //ENGINE 2
-            else if (m_cmd == '4' && steps2 > -242)
+            else if (cmd == '4' && steps2 > -242)
             {
                 rest = false;
                 steps2 --;
                 stepEngine2Left();
             }
-            else if (m_cmd == '6' && steps2 < 242)
+            else if (cmd == '6' && steps2 < 242)
             {
                 rest = false;
                 steps2 ++;
                 stepEngine2Right();
             }
             //ENGINE 3
-            else if (m_cmd == '1' && steps1 > -242)
+            else if (cmd == '1' && steps1 > -242)
             {
                 rest = false;
                 steps1--;
                 stepEngine1Left();
             }
-            else if (m_cmd == '3' && steps1 < 242)
+            else if (cmd == '3' && steps1 < 242)
             {
                 rest = false;
                 steps1++;
                 stepEngine1Right();
             }
             
+            if (m_cmd == 'y')
+            {
+                int step = 100;
+                while (step >= 0)
+                {
+                    step--;
+                    step2Left3Right();
+                }
+                m_cmd = 'x';
+                
+            }
+            
+            if (m_cmd == 't')
+            {
+                int step1, step2, step3;
+                step1 = 100;
+                step2 = 60;
+                step3 = 70;
+                
+                while (step1 > 0)
+                {
+                    stepEngine1Left();
+                    step1--;
+                    //global stepcounter
+                    steps1--;
+                }
+                while(step2 > 0)
+                {
+                    stepEngine2Right();
+                    step2--;
+                    //global stepcounter
+                    steps2++;
+                }
+                while(step3 > 0)
+                {
+                    stepEngine3Left();
+                    step3--;
+                    //global stepcounter
+                    steps3--;
+                }
+                
+                m_cmd = 'x';    
+                
+            }
 
 
 
@@ -740,16 +892,3 @@
         }  //END WHILE TRUE
             
 } //END Main
-
-
-
-
-
-
-
-
-
-
-
-
-