File content as of revision 1:49bfb0a87fbc:

#include "drive.h"
#include "mbed.h"

PwmOut MotorL_EN(P1_19);
DigitalOut MotorL_FORWARD(P2_15);
DigitalOut MotorL_REVERSE(P2_14);

PwmOut MotorR_EN(P2_19);
DigitalOut MotorR_FORWARD(P2_20);
DigitalOut MotorR_REVERSE(P2_21);

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); 
    
  MotorR_FORWARD.write(0); 
  MotorR_REVERSE.write(0);
  MotorR_EN.period_us(255);
  MotorR_EN.pulsewidth_us(0); 
}

void BrakeMotL()
{
  MotorL_FORWARD.write(1); 
  MotorL_REVERSE.write(1);
  MotorL_EN.pulsewidth_us(255);  
}

void BrakeMotR()
{
  MotorR_FORWARD.write(1); 
  MotorR_REVERSE.write(1);
  MotorR_EN.pulsewidth_us(255);  
}

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