James Nagendran / Mbed 2 deprecated 4180_final_receiver

Receiver code for SLVM

Dependencies:   mbed-rtos mbed

Robot.cpp@0:fd289b2e6b74, 2014-12-09 (annotated)

Committer:
jnagendran3
Date:
Tue Dec 09 01:15:37 2014 +0000
Revision:
0:fd289b2e6b74
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jnagendran3 0:fd289b2e6b74 1 #include "Robot.h"
jnagendran3 0:fd289b2e6b74 2
jnagendran3 0:fd289b2e6b74 3 Robot::Robot()
jnagendran3 0:fd289b2e6b74 4 {
jnagendran3 0:fd289b2e6b74 5 //Init member variables to null
jnagendran3 0:fd289b2e6b74 6 host = 0;
jnagendran3 0:fd289b2e6b74 7 AckLED = 0;
jnagendran3 0:fd289b2e6b74 8 }
jnagendran3 0:fd289b2e6b74 9
jnagendran3 0:fd289b2e6b74 10 Robot::Robot(Serial *MbedSerial, DigitalOut *ConfirmationLed)
jnagendran3 0:fd289b2e6b74 11 {
jnagendran3 0:fd289b2e6b74 12 //Init member variables
jnagendran3 0:fd289b2e6b74 13 host = MbedSerial;
jnagendran3 0:fd289b2e6b74 14 AckLED = ConfirmationLed;
jnagendran3 0:fd289b2e6b74 15
jnagendran3 0:fd289b2e6b74 16 //Setup serial
jnagendran3 0:fd289b2e6b74 17 host->baud(19200);
jnagendran3 0:fd289b2e6b74 18
jnagendran3 0:fd289b2e6b74 19 }
jnagendran3 0:fd289b2e6b74 20
jnagendran3 0:fd289b2e6b74 21 bool Robot::SendCommand(string arg1)
jnagendran3 0:fd289b2e6b74 22 {
jnagendran3 0:fd289b2e6b74 23 if(host != 0)
jnagendran3 0:fd289b2e6b74 24 {
jnagendran3 0:fd289b2e6b74 25 for(int i =0; i < arg1.length(); i++)
jnagendran3 0:fd289b2e6b74 26 {
jnagendran3 0:fd289b2e6b74 27 host->putc(arg1[i]);
jnagendran3 0:fd289b2e6b74 28 }
jnagendran3 0:fd289b2e6b74 29
jnagendran3 0:fd289b2e6b74 30 host->putc('\r');
jnagendran3 0:fd289b2e6b74 31 return true;
jnagendran3 0:fd289b2e6b74 32 }
jnagendran3 0:fd289b2e6b74 33 else
jnagendran3 0:fd289b2e6b74 34 {
jnagendran3 0:fd289b2e6b74 35 return false;
jnagendran3 0:fd289b2e6b74 36 }
jnagendran3 0:fd289b2e6b74 37 }
jnagendran3 0:fd289b2e6b74 38
jnagendran3 0:fd289b2e6b74 39 bool Robot::SetMotorVelocity(int velocity)
jnagendran3 0:fd289b2e6b74 40 {
jnagendran3 0:fd289b2e6b74 41 char VelocityCommand[25];
jnagendran3 0:fd289b2e6b74 42
jnagendran3 0:fd289b2e6b74 43 sprintf(VelocityCommand, "mogo 1:%d 2:%d", velocity, velocity);
jnagendran3 0:fd289b2e6b74 44
jnagendran3 0:fd289b2e6b74 45 return SendCommand(VelocityCommand);
jnagendran3 0:fd289b2e6b74 46 }
jnagendran3 0:fd289b2e6b74 47
jnagendran3 0:fd289b2e6b74 48 bool Robot::SetSelectMotorVelocity(int LEFT, int RIGHT)
jnagendran3 0:fd289b2e6b74 49 {
jnagendran3 0:fd289b2e6b74 50 char VelocityCommand[25];
jnagendran3 0:fd289b2e6b74 51
jnagendran3 0:fd289b2e6b74 52 sprintf(VelocityCommand, "mogo 1:%d 2:%d", LEFT, RIGHT);
jnagendran3 0:fd289b2e6b74 53
jnagendran3 0:fd289b2e6b74 54 return SendCommand(string(VelocityCommand));
jnagendran3 0:fd289b2e6b74 55 }
jnagendran3 0:fd289b2e6b74 56
jnagendran3 0:fd289b2e6b74 57 bool Robot::StopRobot()
jnagendran3 0:fd289b2e6b74 58 {
jnagendran3 0:fd289b2e6b74 59 return SendCommand(string("stop"));
jnagendran3 0:fd289b2e6b74 60 }
jnagendran3 0:fd289b2e6b74 61
jnagendran3 0:fd289b2e6b74 62 bool Robot::SetMotorPWM(int ramp, int PWMValue)
jnagendran3 0:fd289b2e6b74 63 {
jnagendran3 0:fd289b2e6b74 64 char PWMCommand[25];
jnagendran3 0:fd289b2e6b74 65
jnagendran3 0:fd289b2e6b74 66 if( ramp > 0)
jnagendran3 0:fd289b2e6b74 67 sprintf(PWMCommand, "pwm r:%d 1:%d 2:%d", ramp, PWMValue, PWMValue);
jnagendran3 0:fd289b2e6b74 68 else
jnagendran3 0:fd289b2e6b74 69 sprintf(PWMCommand, "pwm 1:%d 2:%d", PWMValue, PWMValue);
jnagendran3 0:fd289b2e6b74 70
jnagendran3 0:fd289b2e6b74 71 return SendCommand(string(PWMCommand));
jnagendran3 0:fd289b2e6b74 72 }
jnagendran3 0:fd289b2e6b74 73
jnagendran3 0:fd289b2e6b74 74
jnagendran3 0:fd289b2e6b74 75 bool Robot::SetSelectMotorPWM(int MotorNumber,int ramp, int PWMValue)
jnagendran3 0:fd289b2e6b74 76 {
jnagendran3 0:fd289b2e6b74 77 char PWMCommand[25];
jnagendran3 0:fd289b2e6b74 78
jnagendran3 0:fd289b2e6b74 79 if( ramp > 0)
jnagendran3 0:fd289b2e6b74 80 sprintf(PWMCommand, "pwm r:%d %d:%d", ramp, MotorNumber, PWMValue);
jnagendran3 0:fd289b2e6b74 81 else
jnagendran3 0:fd289b2e6b74 82 sprintf(PWMCommand, "pwm %d:%d", MotorNumber, PWMValue);
jnagendran3 0:fd289b2e6b74 83
jnagendran3 0:fd289b2e6b74 84 return SendCommand(string(PWMCommand));
jnagendran3 0:fd289b2e6b74 85 }
jnagendran3 0:fd289b2e6b74 86
jnagendran3 0:fd289b2e6b74 87 bool Robot::WaitForAck()
jnagendran3 0:fd289b2e6b74 88 {
jnagendran3 0:fd289b2e6b74 89 char Output[4];
jnagendran3 0:fd289b2e6b74 90 int i=0;
jnagendran3 0:fd289b2e6b74 91
jnagendran3 0:fd289b2e6b74 92 while(!host->readable()) {;} //spin unto we get something
jnagendran3 0:fd289b2e6b74 93
jnagendran3 0:fd289b2e6b74 94 //Get all data out of buffer
jnagendran3 0:fd289b2e6b74 95 while(host->readable()) { Output[i++] = (host->getc());
jnagendran3 0:fd289b2e6b74 96
jnagendran3 0:fd289b2e6b74 97 if(i == 4) break;
jnagendran3 0:fd289b2e6b74 98 }
jnagendran3 0:fd289b2e6b74 99
jnagendran3 0:fd289b2e6b74 100 //Return true only if we have an ACK
jnagendran3 0:fd289b2e6b74 101 if(Output[0] == 'A' && Output[1] == 'C')
jnagendran3 0:fd289b2e6b74 102 return true;
jnagendran3 0:fd289b2e6b74 103 else
jnagendran3 0:fd289b2e6b74 104 return false;
jnagendran3 0:fd289b2e6b74 105 }