Diff: Robot.cpp

Revision:

0:fd289b2e6b74

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,105 @@
+#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;
+}