File content as of revision 0:fd289b2e6b74:

#include "Robot.h"

Robot::Robot()
{
    //Init member variables to null
    host   = 0;
    AckLED = 0;
}

Robot::Robot(Serial *MbedSerial, DigitalOut *ConfirmationLed)
{
    //Init member variables
    host   = MbedSerial;
    AckLED = ConfirmationLed;
    
    //Setup serial
    host->baud(19200);  

}

bool Robot::SendCommand(string arg1)
{   
    if(host != 0)
    {
       for(int i =0; i < arg1.length(); i++)
       {
          host->putc(arg1[i]);
       }
       
       host->putc('\r');
       return true;
    }
    else
    {
        return false;
    }
}

bool Robot::SetMotorVelocity(int velocity)
{
   char VelocityCommand[25];
   
   sprintf(VelocityCommand, "mogo 1:%d 2:%d", velocity, velocity); 
   
   return SendCommand(VelocityCommand);   
}

bool Robot::SetSelectMotorVelocity(int LEFT, int RIGHT)
{
   char VelocityCommand[25];
   
   sprintf(VelocityCommand, "mogo 1:%d 2:%d", LEFT, RIGHT); 
   
   return SendCommand(string(VelocityCommand));
}

bool Robot::StopRobot()
{
   return SendCommand(string("stop"));
}

bool Robot::SetMotorPWM(int ramp, int PWMValue)
{
    char PWMCommand[25];
    
    if( ramp > 0)
    sprintf(PWMCommand, "pwm r:%d 1:%d 2:%d", ramp, PWMValue, PWMValue); 
    else
    sprintf(PWMCommand, "pwm 1:%d 2:%d", PWMValue, PWMValue); 
    
    return SendCommand(string(PWMCommand));
}

 
bool Robot::SetSelectMotorPWM(int MotorNumber,int ramp, int PWMValue)
{
    char PWMCommand[25];
    
    if( ramp > 0)
    sprintf(PWMCommand, "pwm r:%d %d:%d", ramp, MotorNumber, PWMValue); 
    else
    sprintf(PWMCommand, "pwm %d:%d", MotorNumber, PWMValue); 
    
    return SendCommand(string(PWMCommand));
}

bool Robot::WaitForAck()
{
    char Output[4];
    int i=0;
    
    while(!host->readable()) {;} //spin unto we get something
    
    //Get all data out of buffer
    while(host->readable()) { Output[i++] = (host->getc());
    
        if(i == 4) break; 
     }
     
     //Return true  only if we have an ACK
     if(Output[0] == 'A' && Output[1] == 'C')
        return true;
     else
       return false;
}