Diff: drive.cpp

Revision:

0:6c2f448aaf0d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drive.cpp	Tue May 05 19:03:45 2015 +0000
@@ -0,0 +1,131 @@
+#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;
+
+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 MotInit()
+{ 
+  MotorL_FORWARD.write(0); 
+  MotorL_REVERSE.write(0);
+  MotorL_EN.period_us(255);
+  MotorL_EN.pulsewidth_us(0); 
+  counterL=0;
+  TachoL.rise(&handleL);
+    
+  MotorR_FORWARD.write(0); 
+  MotorR_REVERSE.write(0);
+  MotorR_EN.period_us(255);
+  MotorR_EN.pulsewidth_us(0); 
+  counterR=0; 
+  TachoR.rise(&handleR);
+}
+
+void BrakeMotL()
+{
+  MotorL_FORWARD.write(0); 
+  MotorL_REVERSE.write(0);
+  MotorL_EN.pulsewidth_us(0);  
+}
+
+void BrakeMotR()
+{
+  MotorR_FORWARD.write(0); 
+  MotorR_REVERSE.write(0);
+  MotorR_EN.pulsewidth_us(0);  
+}
+
+void MotL(int aPow)
+{   
+  if( aPow==0 ) 
+  { 
+    BrakeMotL(); 
+    return; 
+  }
+  
+  if( aPow>255 ) 
+    aPow=255;
+  if( aPow<-255 ) 
+    aPow=-255;
+    
+  if( aPow>0 ) 
+  { 
+    MotorL_FORWARD.write(1); 
+    MotorL_REVERSE.write(0);
+    MotorL_EN.pulsewidth_us(aPow);
+  }
+  else 
+  { 
+    MotorL_FORWARD.write(0); 
+    MotorL_REVERSE.write(1);
+    MotorL_EN.pulsewidth_us(-aPow);  
+  }
+}
+
+void MotR(int aPow)
+{
+ if( aPow==0 ) 
+  { 
+    BrakeMotR(); 
+    return; 
+  }
+  
+  if( aPow>255 ) 
+    aPow=255;
+  if( aPow<-255 ) 
+    aPow=-255;
+ 
+  if( aPow>0 ) 
+  { 
+    MotorR_FORWARD.write(1); 
+    MotorR_REVERSE.write(0);
+    MotorR_EN.pulsewidth_us(aPow);
+  }
+  else 
+  { 
+    MotorR_FORWARD.write(0); 
+    MotorR_REVERSE.write(1);
+    MotorR_EN.pulsewidth_us(-aPow);  
+  } 
+}
+
+void MotSL(int aPow, int deg) // aPow: -255...255  deg: 0...20
+{
+    counterL=deg;
+    MotL(aPow);
+}
+
+void MotSR(int aPow, int deg)
+{
+    counterR=deg;
+    MotR(aPow);
+}