Diff: main.cpp

Revision:

4:b084af72f9a6

Parent:

3:86145fd89b45

Child:

5:84e63108a1b6

--- a/main.cpp	Wed Mar 22 06:42:53 2017 +0000
+++ b/main.cpp	Wed Mar 22 07:15:20 2017 +0000
@@ -11,6 +11,7 @@
 PwmOut pwmR(PA_9);//right 
 
 void InitSerial();
+void runMotor(char mot, bool dir, float pwm);
 void front(float xpwmL,float xpwmR);
 void back(float xpwm,float xpwmR);
 void left(float xpwm, float xpwmR);
@@ -47,7 +48,8 @@
          switch(ch)
             {
               case 'w'://Front
-                   front(0.5,0.5);
+                   runMotor('L', 1, 0.5);
+                   runMotor('R', 1, 0.5);
                    serial.printf("Forward\n");
                    lcd.cls();
                    lcd.locate(1,0);
@@ -55,7 +57,8 @@
                    break;
                   
               case 'a'://Left
-                   left(0,0.5);
+                   runMotor('L', 1, 0);
+                   runMotor('R', 1, 0.5);
                    serial.printf("Left\n");
                    lcd.cls();
                    lcd.locate(1,0);
@@ -63,7 +66,8 @@
                    break; 
                   
               case 's'://Back
-                   back(0.5,0.5);
+                   runMotor('L', 0, 0.5);
+                   runMotor('R', 0, 0.5);
                    serial.printf("Back\n");
                    lcd.cls();
                    lcd.locate(1,0);
@@ -71,13 +75,32 @@
                    break; 
                   
               case 'd'://Right
-                   right(0.5,0);
+                   runMotor('L', 1, 0.5);
+                   runMotor('R', 1, 0);
                    serial.printf("Right\n");
                    lcd.cls();
                    lcd.locate(1,0);
                    lcd.printf("Right\n");
                    break;
                   
+              case 'A'://Anticlock
+                   runMotor('L', 0, 0.5);
+                   runMotor('R', 1, 0.5);
+                   serial.printf("AntiClock\n");
+                   lcd.cls();
+                   lcd.locate(1,0);
+                   lcd.printf("AntiClock\n");
+                   break; 
+                  
+              case 'C'://Clock
+                   runMotor('L', 1, 0.5);
+                   runMotor('R', 0, 0.5);
+                   serial.printf("Clock\n");
+                   lcd.cls();
+                   lcd.locate(1,0);
+                   lcd.printf("Clock\n");
+                   break;
+          
               case 'O'://Stop
                    Brake();
                    serial.printf("STOP\n");
@@ -85,23 +108,7 @@
                    lcd.locate(1,0);
                    lcd.printf("STOP\n");
                    break;
-                  
-              case 'A'://Anticlock
-                   antiClock(0.5,0.5);
-                   serial.printf("AntiClock\n");
-                   lcd.cls();
-                   lcd.locate(1,0);
-                   lcd.printf("AntiClock\n");
-                   break; 
-                  
-              case 'C'://Clock
-                   clockWise(0.5,0.5);
-                   serial.printf("Clock\n");
-                   lcd.cls();
-                   lcd.locate(1,0);
-                   lcd.printf("Clock\n");
-                   break;
-              
+        
               default:
                    Brake();
                    serial.printf("Brake");
@@ -112,6 +119,42 @@
         }
     }
 }
+void runMotor(char mot, bool dir, float pwm)
+{
+    switch(mot)
+    {
+     case 'L':
+      if(dir)
+        {
+         dirLa = 1;
+         dirLb = 0;
+        }
+      else
+        {
+         dirLa = 0;
+         dirLb = 1;
+        }
+      pwmL= pwm;
+      break;
+     
+     case 'R':
+       if(dir)
+        {
+         dirRa = 1;
+         dirRb = 0;
+        }
+      else
+        {
+         dirRa = 0;
+         dirRb = 1;
+        }
+      pwmR= pwm;
+      break;
+      
+      default: 
+      Brake(); 
+     }
+}    
 void front(float xpwmL,float xpwmR)
 {  
     dirLa = 1;
@@ -154,28 +197,31 @@
 }
 void Brake()
 {
-    dirRa = 0;
-    dirRb = 0;
     dirLa = 0;
     dirLb = 0;
     pwmL=0.0;
+    
+    dirRa = 0;
+    dirRb = 0;
     pwmR=0.0; 
 }
 void antiClock(float xpwmL,float xpwmR)
 {
-    dirRa = 1;            
-    dirRb = 0;   
     dirLa = 0;
     dirLb = 1;
     pwmL = xpwmL;
+    
+    dirRa = 1;            
+    dirRb = 0;   
     pwmR = xpwmR;  
 }
 void clockWise(float xpwmL,float xpwmR)
 {
-    dirRa = 0;
-    dirRb = 1;
     dirLa = 1;
     dirLb = 0;
     pwmL = xpwmL;
+    
+    dirRa = 0;
+    dirRb = 1;
     pwmR = xpwmR;
 }
\ No newline at end of file