/// <summary>
/// Executes Misty DriveArc() command but checks for hazards first. Will wait till hazard is resolved to issue command.
/// </summary>
/// <param name="heading">Misty's end rotation will be facing in this direction. Values are degrees from 0-360, but values outside this range will automatically be translated</param>
/// <param name="radius">Radius of the arc that Misty will drive on</param>
/// <param name="timeMs">The amount of time that Misty will drive for</param>
/// <param name="reverse">Not actually sure what this does. Drives in reverse? </param>
/// <param name="commandCallback">Sets the callback function for the Drive command. Null is okay.</param>
/// <param name="wasCalledFromHazard">By default this is false. Only use if calling from a hazard state</param>
public void DriveArc(double heading, double radius, int timeMs, bool reverse, ProcessCommandResponse commandCallback, bool wasCalledFromHazard = false)
{
if (isMovingFromHazard == true && wasCalledFromHazard == false)
{
autoEvent.WaitOne();
}
_misty.DriveArc(heading, radius, timeMs, reverse, null); //commandCallback == null ? null : commandCallback
//if (commandCallback != null)
//{
// _misty.DriveArc(heading, radius, timeMs, reverse, commandCallback);/
//else
//{
// _misty.DriveArc(heading, radius, timeMs, reverse, null);
//}
}
/// <summary>
/// Turn N degrees at medium speed.
/// Confirm with IMU values and retry if needed.
/// </summary>
/// <param name="degrees"></param>
/// <returns></returns>
public async Task <bool> TurnAsync(double degrees)
{
if (Math.Abs(degrees) < 2)
{
return(true);
}
double initialYaw = ImuYaw;
bool success = true;
try
{
// Normalize degrees to be between -180 and 180.
degrees = degrees % 360;
if (degrees > 180)
{
degrees = 360 - degrees;
}
else if (degrees < -180)
{
degrees = 360 + degrees;
}
// Get medium speed duration.
int duration = (int)(500 + Math.Abs(degrees) * 4000.0 / 90.0);
// Send command
LogMessage($"Sending turn command for {degrees:f2} degrees in {duration} ms.");
_misty.DriveArc(ImuYaw + degrees, 0, duration, false, OnResponse);
await Task.Delay(2000);
// Turns less then about 3 degrees don't do anything.
// So in that case don't bother checking or waiting any longer.
if (Math.Abs(degrees) > 3)
{
// Make sure that the command worked and we are turning.
int retries = 0;
while (retries++ < 3 && Math.Abs(AngleDelta(ImuYaw, initialYaw)) < 1.0)
{
LogMessage($"Sending turn command for {degrees:f2} degrees in {duration} ms.");
_misty.DriveArc(ImuYaw + degrees, 0, duration, false, OnResponse);
await Task.Delay(1000);
if (_abort)
{
return(false);
}
}
// Wait for turn to complete.
retries = 0;
double yawBefore = ImuYaw + 500;
while (Math.Abs(AngleDelta(ImuYaw, yawBefore)) > 1.0 && retries++ < 20)
{
yawBefore = ImuYaw;
await Task.Delay(500);
if (_abort)
{
return(false);
}
}
await Task.Delay(250); // A little extra padding for last degree.
}
}
catch (Exception ex)
{
success = false;
LogMessage("Exception occurred within TurnAsync: " + ex.Message);
}
LogMessage($"Completed turn of {AngleDelta(ImuYaw, initialYaw):f2} degrees.");
return(success);
}
/// <summary>
/// Turn N degrees at medium speed.
/// Confirm with IMU values and retry if needed.
/// </summary>
/// <param name="degrees"></param>
/// <returns></returns>
public async Task <bool> TurnAsync(double degrees)
{
if (DateTime.Now.Subtract(LastImuMessageReceived).TotalSeconds > 1)
{
LogMessage($"Cannot carry out a turn command because IMU messages are not being received. Last IMU message received at {LastImuMessageReceived}.");
MistySpeak("IMU messages are not being received. Path following aborted.");
return(false);
}
// Normalize degrees to be between -180 and 180.
degrees = degrees % 360;
if (degrees > 180)
{
degrees = degrees - 360;
}
else if (degrees < -180)
{
degrees = 360 + degrees;
}
if (Math.Abs(degrees) < 2)
{
// Can't turn that little.
return(true);
}
double initialYaw = ImuYaw;
bool success = true;
try
{
// Get medium speed duration.
int duration = (int)(500 + Math.Abs(degrees) * 4000.0 / 90.0);
// Send command
LogMessage($"Sending turn command for {degrees:f1} degrees in {duration} ms.");
_misty.DriveArc(ImuYaw + degrees, 0, duration, false, OnResponse);
await Task.Delay(2000);
if (_abort)
{
return(false);
}
// Turns less then about 3 degrees don't do anything.
// So in that case don't bother checking or waiting any longer.
if (Math.Abs(degrees) > 3)
{
// Make sure that the command worked and we are turning.
int retries = 0;
while (retries++ < 3 && Math.Abs(AngleDelta(ImuYaw, initialYaw)) < 1.0)
{
LogMessage($"Sending turn command for {degrees:f1} degrees in {duration} ms.");
_misty.DriveArc(ImuYaw + degrees, 0, duration, false, OnResponse);
await Task.Delay(1000);
if (_abort)
{
return(false);
}
}
// Wait for turn to complete.
retries = 0;
double yawBefore = ImuYaw + 180;
while (Math.Abs(AngleDelta(yawBefore, ImuYaw)) > 1.0 && retries++ < 25)
{
yawBefore = ImuYaw;
await Task.Delay(500);
if (_abort)
{
return(false);
}
}
await Task.Delay(250); // A little extra padding for last degree.
}
}
catch (Exception ex)
{
success = false;
LogMessage("Exception occurred within TurnAsync: " + ex.Message);
}
double turned = AngleDelta(initialYaw, ImuYaw);
if (Math.Abs(AngleDelta(turned, degrees)) > 5)
{
success = false;
LogMessage($"Only turned {turned:f1} degrees. Expected {degrees:f1} degrees.");
}
else
{
success = true;
LogMessage($"Completed turn of {turned:f1} degrees.");
}
return(success);
}
/// <summary>
/// Redo the commands in the queue in reverse to retrace steps
/// </summary>
/// <param name="_misty">The IRobotMessenger representing the current Misty in use</param>
/// <param name="stepsToRetrace">Number of commands in the queue to retrace</param>
public void RetraceSteps(IRobotMessenger _misty, int stepsToRetrace = -1)
{
HazardSettings hazardSettings = new HazardSettings();
hazardSettings.DisableTimeOfFlights = true;
hazardSettings.DisableBumpSensors = true;
_misty.UpdateHazardSettings(hazardSettings, null);
currentTime = DateTimeOffset.Now;
retracingSteps = true;
if (stepsToRetrace >= moveQueue.Count || stepsToRetrace == -1)
{
stepsToRetrace = moveQueue.Count;
}
//int size = inputBuffer.Count;
Debug.WriteLine("size: " + size);
Debug.WriteLine("stepstoretrace: " + stepsToRetrace);
for (int i = 0; i < stepsToRetrace; i++) // TODO: maybe change the <= to <
{
IRobotCommandEvent moveCommand = Pop();
TimeSpan millisecondsToDriveFor = currentTime.Subtract(moveCommand.Created);
currentTime = moveCommand.Created;
if (moveCommand.Command == "Drive" || moveCommand.Command == "DriveAsync")
{
var linearVelocityString = moveCommand.Parameters["LinearVelocity"];
var angularVelocityString = moveCommand.Parameters["AngularVelocity"];
double linearVelocity = Convert.ToDouble(linearVelocityString);
double angularVelocity = Convert.ToDouble(angularVelocityString);
Debug.WriteLine("MoveCommand[" + i + "] Drive() Linear Velocity: " + (double)linearVelocity * -1 + ", Angular Velocity: " + (double)angularVelocity * -1 + " , ms: " + (int)millisecondsToDriveFor.TotalMilliseconds);
_misty.DriveTime(linearVelocity * -1, angularVelocity * -1, (int)millisecondsToDriveFor.TotalMilliseconds, DriveTrackResponse);
Thread.Sleep((int)millisecondsToDriveFor.TotalMilliseconds + 500);
}
else if (moveCommand.Command == "DriveTime" || moveCommand.Command == "DriveTimeAsync")
{
var linearVelocity = Convert.ToDouble(moveCommand.Parameters["LinearVelocity"]);
var angularVelocity = Convert.ToDouble(moveCommand.Parameters["AngularVelocity"]);
var timeMs = (int)Convert.ToInt64(moveCommand.Parameters["TimeMs"]);
Debug.WriteLine("MoveCommand[" + i + "] DrivTime() Linear Velocity: " + (double)linearVelocity * -1 + ", Angular Velocity: " + (double)angularVelocity * -1 + " , ms: " + (int)millisecondsToDriveFor.TotalMilliseconds);
_misty.DriveTime(linearVelocity * -1, angularVelocity * -1, (int)millisecondsToDriveFor.TotalMilliseconds, DriveTrackResponse);
Thread.Sleep((int)millisecondsToDriveFor.TotalMilliseconds + 500);
}
else if (moveCommand.Command == "Stop" || moveCommand.Command == "StopAsync")
{
Debug.WriteLine("MoveCommand[" + i + "] Stop() Linear Velocity: 0, Angular Velocity: 0 , ms: " + (int)millisecondsToDriveFor.TotalMilliseconds);
_misty.DriveTime(0, 0, (int)millisecondsToDriveFor.TotalMilliseconds, DriveTrackResponse);
Thread.Sleep((int)millisecondsToDriveFor.TotalMilliseconds + 500);
}
else if (moveCommand.Command == "DriveArc" || moveCommand.Command == "DriveArcAsync")
{
var heading = Convert.ToDouble(moveCommand.Parameters["Heading"]);
var radius = Convert.ToDouble(moveCommand.Parameters["Radius"]);
var timeMs = (int)Convert.ToInt64(moveCommand.Parameters["TimeMs"]);
var reverse = Convert.ToBoolean(moveCommand.Parameters["Reverse"]);
Debug.WriteLine("MoveCommand[" + i + "] DriveArc() Heading: " + heading + ", Radius: " + radius + ", TimeMs: " + timeMs + ", Reverse: " + reverse);
_misty.DriveArc(heading, radius, timeMs, !reverse, DriveTrackResponse);
Thread.Sleep(timeMs + 500);
}
Debug.WriteLine("int i = " + i + ", stepstoretrascce = " + stepsToRetrace);
}
hazardSettings.DisableTimeOfFlights = false;
hazardSettings.RevertToDefault = true;
_misty.UpdateHazardSettings(hazardSettings, null);
retracingSteps = false;
}
