Robot Bachelor / Mbed 2 deprecated MC

Engine control program with 3 engines Needs to make a case for each simultaneous engine setting, because of the WAIT after each number of bits have been sent.

Dependencies:   mbed-rtos mbed PinDetect

Fork of FinalMotorControl by Robot Bachelor

Diff: main.cpp

Revision:
7:ba0caeb296bb
Parent:
6:ac5d277bd497
Child:
8:5703dadaed07
--- a/main.cpp	Wed Apr 20 12:11:51 2016 +0000
+++ b/main.cpp	Wed Apr 20 14:23:51 2016 +0000
@@ -4,63 +4,76 @@
 Serial pc(USBTX, USBRX); // tx, rx
 
 //Analog Pins
-//First engine
+//First engine 1-4
 DigitalOut IN1(A0);
 DigitalOut IN2(A1);
 DigitalOut IN3(A2);
 DigitalOut IN4(A3);
-//Second engine
+//Second engine 5-8
 DigitalOut IN5(D3);
 DigitalOut IN6(D4);
 DigitalOut IN7(D5);
 DigitalOut IN8(D6);
+//Third engine 9-12
+DigitalOut IN9(D8);
+DigitalOut IN10(D9);
+DigitalOut IN11(D10);
+DigitalOut IN12(D11);
 
 
 static int fart = 1000;
 static char m_cmd = 'x';
 
-
-
-void step4Right()
+void step4Right3()
 {
-    //engine 1
-    IN1=0;
-    IN2=1;
-    IN3=0;
-    IN4=1;
+
+   //engine 2
+    IN9=0;
+    IN10=1;
+    IN11=0;
+    IN12=1;
+    wait_us(fart);
+    //engine 2
+    IN9=0;
+    IN10=1;
+    IN11=1;
+    IN12=0;
+    wait_us(fart);
     //engine 2
+    IN9=1;
+    IN10=0;
+    IN11=1;
+    IN12=0;
+    wait_us(fart);
+    //engine 2
+    IN9=1;
+    IN10=0;
+    IN11=0;
+    IN12=1;
+    wait_us(fart);
+}
+
+void step4Right2()
+{
+
+   //engine 2
     IN5=0;
     IN6=1;
     IN7=0;
     IN8=1;
-    wait_us(fart); //legg som global variabel "fart" 
-    //engine 1
-    IN1=0;
-    IN2=1;
-    IN3=1;
-    IN4=0;
+    wait_us(fart);
     //engine 2
     IN5=0;
     IN6=1;
     IN7=1;
     IN8=0;
     wait_us(fart);
-    //engine 1
-    IN1=1;
-    IN2=0;
-    IN3=1;
-    IN4=0;
     //engine 2
     IN5=1;
     IN6=0;
     IN7=1;
     IN8=0;
     wait_us(fart);
-    //engine 1
-    IN1=1;
-    IN2=0;
-    IN3=0;
-    IN4=1;
     //engine 2
     IN5=1;
     IN6=0;
@@ -68,51 +81,130 @@
     IN8=1;
     wait_us(fart);
 }
-void step4Left()
+void step4Right1()
 {
     //engine 1
-    IN1=1;
-    IN2=0;
+    IN1=0;
+    IN2=1;
     IN3=0;
     IN4=1;
-    //engine 2
-    IN5=1;
-    IN6=0;
-    IN7=0;
-    IN8=1;
+    wait_us(fart); //legg som global variabel "fart" 
+    //engine 1
+    IN1=0;
+    IN2=1;
+    IN3=1;
+    IN4=0;
     wait_us(fart);
     //engine 1
     IN1=1;
     IN2=0;
     IN3=1;
     IN4=0;
+    wait_us(fart);
+    //engine 1
+    IN1=1;
+    IN2=0;
+    IN3=0;
+    IN4=1;
+    wait_us(fart);
+}
+
+void step4Left3()
+{
+    //engine 2
+    IN9=1;
+    IN10=0;
+    IN11=0;
+    IN12=1;
+    wait_us(fart);
+
+    
+    //engine 2
+    IN9=1;
+    IN10=0;
+    IN11=1;
+    IN12=0;
+    wait_us(fart);
+    
+    //engine 2
+    IN9=0;
+    IN10=1;
+    IN11=1;
+    IN12=0;
+    wait_us(fart);
+    
+    //engine 2
+    IN9=0;
+    IN10=1;
+    IN11=0;
+    IN12=1;
+    wait_us(fart);
+    
+}
+
+
+void step4Left2()
+{
+    //engine 2
+    IN5=1;
+    IN6=0;
+    IN7=0;
+    IN8=1;
+    wait_us(fart);
+
+    
     //engine 2
     IN5=1;
     IN6=0;
     IN7=1;
     IN8=0;
     wait_us(fart);
+    
+    //engine 2
+    IN5=0;
+    IN6=1;
+    IN7=1;
+    IN8=0;
+    wait_us(fart);
+    
+    //engine 2
+    IN5=0;
+    IN6=1;
+    IN7=0;
+    IN8=1;
+    wait_us(fart);
+    
+}
+
+void step4Left1()
+{
+    //engine 1
+    IN1=1;
+    IN2=0;
+    IN3=0;
+    IN4=1;
+
+    wait_us(fart);
+    //engine 1
+    IN1=1;
+    IN2=0;
+    IN3=1;
+    IN4=0;
+
+    wait_us(fart);
     //engine 1
     IN1=0;
     IN2=1;
     IN3=1;
     IN4=0;
-    //engine 2
-    IN5=0;
-    IN6=1;
-    IN7=1;
-    IN8=0;
+
     wait_us(fart);
     //engine 1
     IN1=0;
     IN2=1;
     IN3=0;
     IN4=1;
-    //engine 2
-    IN5=0;
-    IN6=1;
-    IN7=0;
-    IN8=1;
+
     wait_us(fart);
 }
 
@@ -144,12 +236,20 @@
         {
             //Thread::wait(5);
             
+            if (m_cmd == 't')
+            
+            {
+                step4Left1();
+            }
+            
             //0 STEPS LEFT AT MAX SPEED
             if (m_cmd == '0')
             {
                 fart = 1000;
                 //global_direction = '0';
-                step4Left();
+                step4Left1();
+                step4Left2();
+                step4Left3();t
                 //steps = steps + step;
                 
                 
@@ -161,7 +261,9 @@
             {
                 fart = 1000;
                 //global_direction = '1';
-                step4Right(); 
+                step4Right1();
+                step4Right2(); 
+                step4Right3();
             } 
                 else
             {
@@ -172,7 +274,9 @@
             {
                 fart =  2000;
                 //global_direction = '1';
-                step4Right();
+                step4Right1();
+                step4Right2(); 
+                step4Right3();
                 //steps = steps + step;
                // printf("%i", &steps);
                 
@@ -182,7 +286,9 @@
             {
                 fart = 2000;
                 //global_direction = '0';
-                step4Left();
+                step4Left1();
+                step4Left2();
+                step4Left3();
                 
                 //steps = steps + step;
                // printf("%i", &steps);
@@ -195,7 +301,9 @@
                 //Steps = 242 gives a 60' rotation to the left. 
                 while (steps >= 0)
                 {
-                    step4Left();
+                    step4Left1();
+                    step4Left2();
+                    step4Left3();
                     steps--;
                 }
                 m_cmd = 'x';
@@ -206,7 +314,9 @@
                 //Steps = 242 gives a 60' rotation to the right. 
                 while (steps >= 0)
                 {
-                    step4Right();
+                    step4Right1();
+                    step4Right2(); 
+                    step4Right3();
                     steps--;
                 }
                 m_cmd = 'x';