File content as of revision 0:5dcb55a2880c:

#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);
}