drive.cpp

00001 #include "drive.h"
00002 #include "mbed.h"
00003 
00004 PwmOut MotorL_EN(P1_15);
00005 DigitalOut MotorL_FORWARD(P1_1);
00006 DigitalOut MotorL_REVERSE(P1_0);
00007 InterruptIn TachoL(P1_12);
00008 
00009 PwmOut MotorR_EN(P0_21);
00010 DigitalOut MotorR_FORWARD(P1_3);
00011 DigitalOut MotorR_REVERSE(P1_4);
00012 InterruptIn TachoR(P1_13);
00013 
00014 int counterL, counterR;
00015 int turnL, turnR;
00016 
00017 int map(int x, int in_min, int in_max, int out_min, int out_max)
00018 {
00019   long t1 = (x - in_min) * (out_max - out_min);
00020   long t2 = (in_max - in_min) + out_min;
00021   long t3 = t1/t2;
00022   int t4 = t3 + 0.5;
00023   return t4;
00024 }
00025 
00026 int getL()
00027 {
00028     return  counterL;
00029 }
00030 
00031 int getR()
00032 {
00033     return  counterR;
00034 }
00035 
00036 void setL(int val)
00037 {
00038     counterL = val;
00039 }
00040 
00041 void setR(int val)
00042 {
00043     counterR = val;
00044 }
00045 
00046 
00047 void handleL()
00048 {
00049     if (counterL==0)
00050         return;
00051     
00052     counterL--;
00053     if (counterL==0)
00054         MotorL_EN.pulsewidth_us(0);
00055 }
00056 
00057 void handleR()
00058 {
00059     if (counterR==0)
00060         return;
00061     
00062     counterR--;
00063     if (counterR==0)
00064         MotorR_EN.pulsewidth_us(0);
00065 }
00066 
00067 void MotConfig(int tl, int tr)
00068 {
00069     turnL = tl;
00070     turnR = tr;
00071 }
00072 
00073 void MotInit()
00074 { 
00075   MotorL_FORWARD.write(0); 
00076   MotorL_REVERSE.write(0);
00077   MotorL_EN.period_us(255);
00078   MotorL_EN.pulsewidth_us(0); 
00079   counterL=0;
00080   turnL=24;
00081   TachoL.rise(&handleL);
00082     
00083   MotorR_FORWARD.write(0); 
00084   MotorR_REVERSE.write(0);
00085   MotorR_EN.period_us(255);
00086   MotorR_EN.pulsewidth_us(0); 
00087   counterR=0;
00088   turnR=24; 
00089   TachoR.rise(&handleR);
00090 }
00091 
00092 void BrakeMotL()
00093 {
00094   MotorL_FORWARD.write(0); 
00095   MotorL_REVERSE.write(0);
00096   MotorL_EN.pulsewidth_us(0);  
00097 }
00098 
00099 void BrakeMotR()
00100 {
00101   MotorR_FORWARD.write(0); 
00102   MotorR_REVERSE.write(0);
00103   MotorR_EN.pulsewidth_us(0);  
00104 }
00105 
00106 void MotL(int aPow) // aPow: -255...255 
00107 {   
00108   if( aPow==0 ) 
00109   { 
00110     BrakeMotL(); 
00111     return; 
00112   }
00113   
00114   if( aPow>255 ) 
00115     aPow=255;
00116   if( aPow<-255 ) 
00117     aPow=-255;
00118     
00119   if( aPow>0 ) 
00120   { 
00121     MotorL_FORWARD.write(1); 
00122     MotorL_REVERSE.write(0);
00123     MotorL_EN.pulsewidth_us(aPow);
00124   }
00125   if( aPow<0 ) 
00126   { 
00127     MotorL_FORWARD.write(0); 
00128     MotorL_REVERSE.write(1);
00129     MotorL_EN.pulsewidth_us(-aPow);  
00130   }
00131 }
00132 
00133 void MotR(int aPow)
00134 {
00135  if( aPow==0 ) 
00136   { 
00137     BrakeMotR(); 
00138     return; 
00139   }
00140   
00141   if( aPow>255 ) 
00142     aPow=255;
00143   if( aPow<-255 ) 
00144     aPow=-255;
00145  
00146   if( aPow>0 ) 
00147   { 
00148     MotorR_FORWARD.write(1); 
00149     MotorR_REVERSE.write(0);
00150     MotorR_EN.pulsewidth_us(aPow);
00151   }
00152   if( aPow<0 ) 
00153   { 
00154     MotorR_FORWARD.write(0); 
00155     MotorR_REVERSE.write(1);
00156     MotorR_EN.pulsewidth_us(-aPow);  
00157   } 
00158 }
00159 
00160 void MotDegL(int aPow, int deg) // aPow: -255...255  deg: 0...360
00161 {
00162     counterL=map(deg,0,360,0,turnL);
00163     MotL(aPow);
00164 }
00165 
00166 void MotDegR(int aPow, int deg)
00167 {
00168     counterR=map(deg,0,360,0,turnR);
00169     MotR(aPow);
00170 }