/// <summary>
///
/// </summary>
/// <param name="vSpeed"></param>
/// <param name="rAngle"></param>
/// <returns></returns>
public bool Drive(Velocity vSpeed, Radius rAngle)
{
this.ErrorText = "";
bool bSafe = this.Mode == SCI_Mode.Safe;
bool bFull = this.Mode == SCI_Mode.Full;
bool bError = (!bSafe) & (!bFull);
//Push back at the user
if (bError)
{
throw new RoombaException("Roomba must be in Safe or Full mode before running Calling the Drive Function: ");
}
//Sample from the SCI Spec:
//to drive in reverse at a velocity of -200 mm/s while turning at a radius of 500mm, you would send the serial byte sequence
//[137] [255] [56] [1] [244]
//[137] [Velocity High Byte] [Velocity Low Byte] [Radius High Byte] [Radius Low Byte]
//divide up vSpeed & rAngle into 2 bytes ea
int num = rAngle.ToInt;
byte byAngleHi = (byte)(num >> 8);
byte byAngleLo = (byte)(num & 255);
num = vSpeed.ToInt;
byte bySpeedHi = (byte)(num >> 8);
byte bySpeedLo = (byte)(num & 255);
bool bSuccess = false;
List<byte> lSend = new List<byte>();
lSend.Add(OpCode.Drive);
lSend.Add(bySpeedHi);
lSend.Add(bySpeedLo);
lSend.Add(byAngleHi);
lSend.Add(byAngleLo);
string sDebugSend = "[" + lSend[0].ToString() + "][" + lSend[1].ToString() + "][" + lSend[2].ToString() + "][" + lSend[3].ToString() + "][" + lSend[4].ToString() + "]";
try
{
Log.This("Drive Action: " + sDebugSend, c_sRoomba, this.LogSCICommands);
this.IO.RtsEnable = false;
this.IO.Write(lSend.ToArray(), 0, lSend.Count);
this.Macro.SetAction("DRIVE\t" + sDebugSend);
this.p_vVelocity = vSpeed;
this.p_rRadius = rAngle;
bSuccess = true;
Log.This("Drive Action Success " + bSuccess.ToString() + " Velocity: " + this.p_vVelocity.ToString() + " Radius: " + this.p_rRadius.ToString(), c_sRoomba, this.LogSCICommands);
}
catch (Exception ex)
{
Log.This("Drive Action Fail: " + ex.Message, c_sRoomba, this.LogSCICommands);
}
return bSuccess;
}
/// <summary>
///
/// </summary>
/// <param name="vSpeed"></param>
/// <param name="rAngle"></param>
/// <param name="bySCI_Mode"></param>
/// <returns></returns>
public bool Drive(Velocity vSpeed, Radius rAngle, byte bySCI_Mode)
{
bool bDriveSuccess;
this.Mode = bySCI_Mode;
bDriveSuccess = this.Drive(vSpeed, rAngle);
return bDriveSuccess;
}
