Diff: drive.cpp

Revision:

0:5dcb55a2880c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/drive.cpp	Tue Aug 18 09:00:51 2015 +0000
@@ -0,0 +1,170 @@
+#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;
+}
+
+void setL(int val)
+{
+    counterL = val;
+}
+
+void setR(int val)
+{
+    counterR = val;
+}
+
+
+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;
+}
+
+void MotInit()
+{ 
+  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);  
+}
+
+void BrakeMotR()
+{
+  MotorR_FORWARD.write(0); 
+  MotorR_REVERSE.write(0);
+  MotorR_EN.pulsewidth_us(0);  
+}
+
+void MotL(int aPow) // aPow: -255...255 
+{   
+  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);
+  }
+  if( aPow<0 ) 
+  { 
+    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);
+  }
+  if( aPow<0 ) 
+  { 
+    MotorR_FORWARD.write(0); 
+    MotorR_REVERSE.write(1);
+    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);
+}