Diff: drive.cpp

Revision:

1:49bfb0a87fbc

Parent:

0:5dcb55a2880c

--- a/drive.cpp	Tue Aug 18 09:00:51 2015 +0000
+++ b/drive.cpp	Thu Apr 27 08:40:17 2017 +0000
@@ -1,106 +1,42 @@
 #include "drive.h"
 #include "mbed.h"
 
-PwmOut MotorL_EN(P1_15);
-DigitalOut MotorL_FORWARD(P1_1);
-DigitalOut MotorL_REVERSE(P1_0);
-InterruptIn TachoL(P1_12);
-
-PwmOut MotorR_EN(P0_21);
-DigitalOut MotorR_FORWARD(P1_3);
-DigitalOut MotorR_REVERSE(P1_4);
-InterruptIn TachoR(P1_13);
-
-int counterL, counterR;
-int turnL, turnR;
-
-int map(int x, int in_min, int in_max, int out_min, int out_max)
-{
-  long t1 = (x - in_min) * (out_max - out_min);
-  long t2 = (in_max - in_min) + out_min;
-  long t3 = t1/t2;
-  int t4 = t3 + 0.5;
-  return t4;
-}
-
-int getL()
-{
-    return  counterL;
-}
-
-int getR()
-{
-    return  counterR;
-}
+PwmOut MotorL_EN(P1_19);
+DigitalOut MotorL_FORWARD(P2_15);
+DigitalOut MotorL_REVERSE(P2_14);
 
-void setL(int val)
-{
-    counterL = val;
-}
-
-void setR(int val)
-{
-    counterR = val;
-}
-
+PwmOut MotorR_EN(P2_19);
+DigitalOut MotorR_FORWARD(P2_20);
+DigitalOut MotorR_REVERSE(P2_21);
 
-void handleL()
-{
-    if (counterL==0)
-        return;
-    
-    counterL--;
-    if (counterL==0)
-        MotorL_EN.pulsewidth_us(0);
-}
-
-void handleR()
-{
-    if (counterR==0)
-        return;
-    
-    counterR--;
-    if (counterR==0)
-        MotorR_EN.pulsewidth_us(0);
-}
-
-void MotConfig(int tl, int tr)
-{
-    turnL = tl;
-    turnR = tr;
-}
+DigitalOut Von(P2_13);      // 12V on
 
 void MotInit()
 { 
+  Von.write(1);
   MotorL_FORWARD.write(0); 
   MotorL_REVERSE.write(0);
   MotorL_EN.period_us(255);
   MotorL_EN.pulsewidth_us(0); 
-  counterL=0;
-  turnL=24;
-  TachoL.rise(&handleL);
     
   MotorR_FORWARD.write(0); 
   MotorR_REVERSE.write(0);
   MotorR_EN.period_us(255);
   MotorR_EN.pulsewidth_us(0); 
-  counterR=0;
-  turnR=24; 
-  TachoR.rise(&handleR);
 }
 
 void BrakeMotL()
 {
-  MotorL_FORWARD.write(0); 
-  MotorL_REVERSE.write(0);
-  MotorL_EN.pulsewidth_us(0);  
+  MotorL_FORWARD.write(1); 
+  MotorL_REVERSE.write(1);
+  MotorL_EN.pulsewidth_us(255);  
 }
 
 void BrakeMotR()
 {
-  MotorR_FORWARD.write(0); 
-  MotorR_REVERSE.write(0);
-  MotorR_EN.pulsewidth_us(0);  
+  MotorR_FORWARD.write(1); 
+  MotorR_REVERSE.write(1);
+  MotorR_EN.pulsewidth_us(255);  
 }
 
 void MotL(int aPow) // aPow: -255...255 
@@ -156,15 +92,3 @@
     MotorR_EN.pulsewidth_us(-aPow);  
   } 
 }
-
-void MotDegL(int aPow, int deg) // aPow: -255...255  deg: 0...360
-{
-    counterL=map(deg,0,360,0,turnL);
-    MotL(aPow);
-}
-
-void MotDegR(int aPow, int deg)
-{
-    counterR=map(deg,0,360,0,turnR);
-    MotR(aPow);
-}