/// <summary> /// Add a movement command to the queue. /// </summary> /// <param name="moveCommand">Movement command to be added to the queue</param> public void Enqueue(IRobotCommandEvent moveCommand) { if (retracingSteps == true) { return; } Debug.WriteLine("Enqueuing: " + moveCommand.Command); Debug.WriteLine("moveQueue.Count: " + moveQueue.Count); Debug.WriteLine(""); moveQueue.Add(moveCommand); // remove oldest command if queue gets too big. Unlikely while (moveQueue.Count > size) { Debug.WriteLine("queue size too big"); moveQueue.RemoveAt(0); } }
/// <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; }