public void OnStart(object sender, IDictionary <string, object> parameters)
{
Task.Run(async() =>
{
_skillHelper = new SkillHelper(_misty);
await Task.Delay(3000);
// Load the path to follow.
_followPath = new FollowPath(_misty, _skillHelper);
List <IFollowPathCommand> commands = await _followPath.LoadCommandsAsync(PATH_FILE_NAME, MyDelegateAsync);
// Follow the path
await _followPath.DriveAsync(commands);
// Dock
await _skillHelper.DisableHazardSystemAsync();
// Dock
#if MAP_DOCK
bool started = await DockAlignAsync();
if (started)
{
await _skillHelper.TurnAsync(180);
_misty.DriveHeading(0, .6, 3000, true, OnResponse);
await Task.Delay(4000);
_misty.DriveHeading(0, .2, 1000, true, OnResponse);
await Task.Delay(2000);
_misty.PlayAudio("s_Awe.wav", 100, OnResponse);
}
#else
_docker = new ChargerDock(_misty, _skillHelper, HEAD_PITCH_OFFSET, HEAD_ROLL_OFFSET, HEAD_YAW_OFFSET);
await _docker.DockAsync();
#endif
await _skillHelper.EnableHazardSystemAsync();
await Task.Delay(30000);
Cleanup();
});
}
/// <summary>
/// Drive straight for the specified distance in meters at a medium speed.
/// Confirm movement with encoder values and retry if needed.
/// If stopped due to a hazard wait a little bit for it to go away and then continue.
/// </summary>
/// <param name="distance"></param>
/// <param name="slow"></param>
public async Task <bool> DriveAsync(double distance, bool slow = false)
{
if (Math.Abs(distance) < 0.001)
{
return(true);
}
bool success = true;
try
{
// Clear encoder values.
int maxWait = 0;
while (_leftEncoderValue != 0 && maxWait++ < 10)
{
_misty.DriveHeading(0, 0, 100, false, OnResponse);
await Task.Delay(200);
}
if (_leftEncoderValue != 0)
{
LogMessage("Failed to reset encoder values.");
return(false);
}
_leftEncoderValues = new ConcurrentQueue <double>();
_stopHazardLatching = false;
bool driving = true;
bool sendCommand = true;
maxWait = 0;
while (driving)
{
if (sendCommand)
{
// Arbitrary medium speed based upon distance.
int duration = (int)(500 + Math.Abs(distance) * 2500);
if (slow)
{
duration = 2 * duration;
}
LogMessage($"Sending drive command with a distance of {distance:f3} meters and a duration of {duration} ms.");
while (!_leftEncoderValues.IsEmpty) // Clearing the encoder queue
{
_leftEncoderValues.TryDequeue(out double r);
}
_misty.DriveHeading(0, Math.Abs(distance), duration, distance < 0, OnResponse);
sendCommand = false;
await Task.Delay(1000);
}
if (_abort)
{
return(false);
}
await Task.Delay(1000);
// Check that we really moved and if we're done.
// Drive command could have been dropped and/or a hazard could have stopped the robot.
double[] encoderValues = _leftEncoderValues.ToArray();
if (encoderValues.Length > 1) // encoder values should be arriving at 5Hz.
{
double distanceDriven = Math.Abs(encoderValues[encoderValues.Length - 1] / 1000.0);
//LogMessage($"Distance driven: {distanceDriven}.");
if (distanceDriven > Math.Abs(0.99 * distance) ||
Math.Abs(distanceDriven - Math.Abs(distance)) < 0.1)
{
LogMessage($"Completed drive with distance of {_leftEncoderValue / 1000.0:f3} meters.");
driving = false;
}
else
{
// Not there yet. Everything progressing okay?
if (_stopHazardLatching)
{
// We've stopped due to a hazard. Wait a bit for it to go away.
LogMessage("Drive command paused for hazard.");
_misty.PlayAudio("s_anger.wav", 100, OnResponse);
int maxHazardWait = 0;
while (_stopHazardState && maxHazardWait++ < 30)
{
if (_abort)
{
return(false);
}
await Task.Delay(1000);
}
if (_stopHazardState)
{
// Still in hazard state. Give up.
LogMessage("Giving up on drive command due to persistent hazard condition.");
success = false;
driving = false;
}
else
{
// We're out of hazard state.
LogMessage("Out of hazard state.");
distance -= encoderValues[encoderValues.Length - 1] / 1000.0;
_stopHazardLatching = false;
sendCommand = true;
}
}
if (encoderValues.Length > 2 && Math.Abs(encoderValues[encoderValues.Length - 1] - encoderValues[encoderValues.Length - 2]) < 0.0001)
{
// Encoder value not changing. Need to send drive command again.
LogMessage($"Encoder values not changing. Distance driven so far is {distanceDriven}.");
distance -= encoderValues[encoderValues.Length - 1] / 1000.0;
sendCommand = true;
}
}
}
else
{
// Something is wrong if we get here.
// For now, just continue and hope for the best :).
LogMessage("Not receiving encoder messages. Can't verify drive commands.");
}
}
}
catch (Exception ex)
{
LogMessage("Exception occurred within DriveAsync: " + ex.Message);
success = false;
}
finally
{
_leftEncoderValues = null;
}
return(success);
}
private async Task <bool> ExecuteDockAsync(bool lastTry)
{
// Find the charger. It should be right in front of us.
// Depending upon how far away from the charger we are we may need a different head pitch.
// Start by assuming that we're fairly close to the charger which means we need to look more downward to see the charger.
double headPitch = 35;
await _skillHelper.MoveHeadAsync(headPitch, 0, 0);
if (!await FindChargerAsync())
{
headPitch = 15;
await _skillHelper.MoveHeadAsync(headPitch, 0, 0);
if (!await FindChargerAsync())
{
_skillHelper.MistySpeak("I can't find the charger.");
await Task.Delay(3000);
return(false);
}
}
_skillHelper.MistySpeak("I see the charger.");
_skillHelper.LogMessage($"Charger is located at [x, y, yaw] : [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Yaw.Degrees():f1}].");
int retries = 0;
while (_chargerPose.X - IDEAL_ALIGNMENT_DISTANCE > 0.04 || IDEAL_ALIGNMENT_DISTANCE - _chargerPose.X > 0.02 ||
Math.Abs(_chargerPose.Y) > 0.01 || Math.Abs(_chargerPose.Yaw.Degrees()) > 2.5)
{
var goalPose = new Simple2DPose()
{
X = IDEAL_ALIGNMENT_DISTANCE,
Y = 0,
Yaw = 0
};
MoveSequence moveSequence = NavigationHelper.CalculateMoveSequence(_chargerPose, goalPose);
_skillHelper.LogMessage($"Movement sequence to goal is turn {moveSequence.Turn1.Degrees():f0} degrees, drive {moveSequence.DriveDistance:f3} meters, " +
$"and turn {moveSequence.Turn2.Degrees():f0} degrees.");
if (!await _skillHelper.TurnAsync(moveSequence.Turn1.Degrees()))
{
return(false);
}
if (_abort)
{
return(false);
}
if (!await _skillHelper.DriveAsync(moveSequence.DriveDistance, true))
{
return(false);
}
if (_abort)
{
return(false);
}
if (!await _skillHelper.TurnAsync(moveSequence.Turn2.Degrees()))
{
return(false);
}
if (_abort)
{
return(false);
}
if (headPitch != 35)
{
// In case we had to lift the head to originally find the charger.
headPitch = 35;
await _skillHelper.MoveHeadAsync(headPitch, 0, 0);
}
await Task.Delay(1000);
if (!await CanSeeChargerAsync(1))
{
_skillHelper.LogMessage("Can no longer see the charger.");
_skillHelper.MistySpeak("Uh oh. I can't see the charger any more.");
return(false);
}
_skillHelper.LogMessage($"Charger now located at [x, y, yaw] : [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Yaw.Degrees():f1}]. " +
$"Error : [{(_chargerPose.X - goalPose.X):f3}, {(_chargerPose.Y - goalPose.Y):f3}, {(_chargerPose.Yaw.Degrees() - goalPose.Yaw.Degrees()):f1}].");
if (retries++ > 5)
{
_skillHelper.LogMessage("Failed to line up with the charger.");
_skillHelper.MistySpeak("I can't seem to line up right.");
await Task.Delay(5000);
return(false);
}
}
_skillHelper.MistySpeak("I should be all lined up now. Going to turn around and back on to the charger.");
// Turn around.
// Note that we do 2 x 91 degrees. Misty tends to turn slightly less than requested and any turn where the absolute
// value is over 180 degrees will get converted to a <180 turn in the other direction.
_skillHelper.LogMessage("Turning 180 degrees to face away from charger.");
if (!await _skillHelper.TurnAsync(91))
{
return(false);
}
if (_abort)
{
return(false);
}
if (!await _skillHelper.TurnAsync(91))
{
return(false);
}
if (_abort)
{
return(false);
}
// Back on to charger.
_skillHelper.LogMessage($"Driving {IDEAL_ALIGNMENT_DISTANCE:f3} meters to back on to charger.");
if (!await _skillHelper.DriveAsync(-IDEAL_ALIGNMENT_DISTANCE - 0.1, true))
{
return(false);
}
if (_abort)
{
return(false);
}
// Move forward slightly.
_misty.DriveHeading(0, 0.01, 1000, false, OnResponse);
// It can take several seconds for the charging indicator to update...
_charging = false;
DateTime start = DateTime.Now;
while (!_charging && DateTime.Now.Subtract(start).TotalSeconds < 8)
{
await Task.Delay(250);
}
// If charging then the battery current is a positive number about 0.4 Amps
if (_charging)
{
_skillHelper.MistySpeak("Ahh. I feel the power.");
await Task.Delay(2000);
}
else
{
if (!lastTry)
{
_skillHelper.MistySpeak("Hmm. I don't seem to be charging. I'm going to drive forward and try docking again.");
_skillHelper.LogMessage("Did not detect that we're charging. Driving forward and trying again.");
await _skillHelper.DriveAsync(IDEAL_ALIGNMENT_DISTANCE + 0.1);
if (_abort)
{
return(false);
}
await _skillHelper.TurnAsync(180);
return(false);
}
}
return(true);
}
private async Task <bool> ExecuteDockAsync()
{
_charging = false;
// Set head position.
await _skillHelper.MoveHeadAsync(_headPitchOffset, _headRollOffset, _headYawOffset);
if (_abort)
{
return(false);
}
// Find the charger.
if (!await FindChargerAsync())
{
return(false);
}
// Get Misty aligned with charger based upon charger pose X and Euler angle.
int retries = 0;
while (Math.Abs(_chargerPose.X) > ALIGNED_X || Math.Abs(_chargerPose.EulerYaw) > ALIGNED_EULER_YAW)
{
if (retries++ > ALIGN_MAX_RETRIES)
{
_skillHelper.LogMessage("Failed to align with charger.");
return(false);
}
_skillHelper.LogMessage($"Charger position is [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Z:f3}] meters. Euler yaw is {_chargerPose.EulerYaw:f3} degrees.");
await FaceChargerAsync(ALIGNED_X);
if (_abort)
{
return(false);
}
if (Math.Abs(_chargerPose.X) > ALIGNED_X || Math.Abs(_chargerPose.EulerYaw) > ALIGNED_EULER_YAW)
{
await AlignWithChargerAsync();
if (Math.Abs(_chargerPose.Z - IDEAL_ALIGNMENT_DISTANCE) > .1)
{
await _skillHelper.DriveAsync(_chargerPose.Z - IDEAL_ALIGNMENT_DISTANCE);
}
}
}
_skillHelper.LogMessage($"Charger position is [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Z:f3}] meters. Euler yaw is {_chargerPose.EulerYaw:f3} degrees.");
// Backup and drive straight forward to check alignment.
double chargerDistance = _chargerPose.Z;
var offsets = new List <double>();
await _skillHelper.DriveAsync(-1.0, true);
await Task.Delay(1000);
offsets.Add(_chargerPose.X);
for (int i = 0; i < 6; i++)
{
await _skillHelper.DriveAsync(.25, true);
await Task.Delay(1000);
offsets.Add(_chargerPose.X);
}
foreach (var o in offsets)
{
_skillHelper.LogMessage(o.ToString("f3"));
}
double slope = (offsets[0] - offsets.Last()) / 1.5;
double estimateFinalOffset = offsets.Last() - slope * 0.5;
double offsetAngle = -Math.Asin(estimateFinalOffset / 0.4) * 180.0 / Math.PI;
_skillHelper.LogMessage($"Estimate final offset {estimateFinalOffset:f3} meters and {offsetAngle} degrees.");
if (Math.Abs(offsetAngle) > FINAL_OFFSET_ANGLE_MAX)
{
_skillHelper.LogMessage("Offset angle is too large. Backing up and retrying dock process.");
await _skillHelper.DriveAsync(-IDEAL_ALIGNMENT_DISTANCE + 0.25);
return(false);
}
await _skillHelper.TurnAsync(offsetAngle);
// Turn around.
_skillHelper.LogMessage("Turning 180 degrees to face away from charger.");
await _skillHelper.TurnAsync(-180);
if (_abort)
{
return(false);
}
// Back on to charger.
_skillHelper.LogMessage($"Driving {chargerDistance - 0.5 + CHARGER_DOCK_OVERSHOOT:f3} meters to back on to charger.");
await _skillHelper.DriveAsync(-chargerDistance + 0.5 - CHARGER_DOCK_OVERSHOOT, true);
if (_abort)
{
return(false);
}
// Check if we've ended up on top of the wedge.
await Task.Delay(1000);
_skillHelper.LogMessage($"Roll = {_skillHelper.ImuRoll:f3}. Pitch = {_skillHelper.ImuPitch:f3}.");
if (Math.Abs(_skillHelper.ImuRoll) > MISTY_ON_WEDGE_ROLL || Math.Abs(AxisRotation(_initialPitch, _skillHelper.ImuPitch)) > MISTY_ON_WEDGE_PITCH)
{
// We're on the wedge. Drive away from the charger and try again.
_skillHelper.LogMessage($"We appear to have driven on top of the alignment wedge. IMU roll is {_skillHelper.ImuRoll:f3}. IMU pitch is {_skillHelper.ImuPitch:f3}.");
await _skillHelper.DriveAsync(IDEAL_ALIGNMENT_DISTANCE - 0.1);
if (_abort)
{
return(false);
}
await _skillHelper.TurnAsync(180);
_chargerPose = null;
return(false);
}
// Check that we're fully docked: back up a little bit to get more aligned.
_misty.DriveHeading(0, 0.2, 500, true, OnResponse);
// It can take several seconds for the charging indicator to update...
await Task.Delay(7000);
// Check that we're charging.
if (!_charging)
{
_misty.PlayAudio("s_Anger3.wav", 100, OnResponse);
await _skillHelper.DriveAsync(IDEAL_ALIGNMENT_DISTANCE - 0.1);
if (_abort)
{
return(false);
}
await _skillHelper.TurnAsync(180);
_chargerPose = null;
return(false);
}
_misty.PlayAudio("s_Ecstacy.wav", 100, OnResponse);
return(true);
}
/// <summary>
/// Drive straight for the specified distance in meters at a medium speed.
/// Confirm movement with encoder values and retry if needed.
/// If stopped due to a hazard wait a little bit for it to go away and then continue.
/// </summary>
/// <param name="distance"></param>
/// <param name="slow"></param>
public async Task <bool> DriveAsync(double distance, bool slow = false)
{
if (Math.Abs(distance) < 0.003)
{
// Can't drive this short a distance
return(true);
}
if (DateTime.Now.Subtract(LastEncoderMessageReceived).TotalSeconds > 1)
{
LogMessage($"Cannot carry out a drive command because encoder messages are not being received. Last encoder message received at {LastEncoderMessageReceived}.");
MistySpeak("Encoder messages are not being received. Path following aborted.");
return(false);
}
bool success = true;
try
{
// Clear encoder values by sending a drive command with a distance of 0.
int encoderResets = 0;
while (_leftEncoderValue != 0 && encoderResets++ < 10)
{
_misty.DriveHeading(0, 0, 100, false, OnResponse);
await Task.Delay(500);
}
if (_leftEncoderValue != 0)
{
LogMessage($"Failed to reset encoder values. Last encoder value received at {LastEncoderMessageReceived} with a value of {_leftEncoderValue}.");
return(false);
}
_leftEncoderValues = new ConcurrentQueue <double>();
_stopHazardLatching = false;
bool driving = true;
bool sendCommand = true;
int duration = 0;
int retries = 0;
DateTime start = DateTime.Now;
while (driving)
{
if (sendCommand)
{
// Arbitrary medium speed based upon distance.
duration = (int)(500 + Math.Abs(distance) * 3000);
if (slow)
{
duration = 3 * duration;
}
while (!_leftEncoderValues.IsEmpty) // Clearing the encoder queue
{
_leftEncoderValues.TryDequeue(out double r);
}
LogMessage($"Sending drive command with a distance of {distance:f3} meters and a duration of {duration} ms.");
_misty.DriveHeading(0, Math.Abs(distance), duration, distance < 0, OnResponse);
start = DateTime.Now;
sendCommand = false;
await Task.Delay(1000);
}
if (_abort)
{
return(false);
}
await Task.Delay(1500);
// Check that we really moved and if we're done.
// Drive command could have been dropped and/or a hazard could have stopped the robot.
double[] encoderValues = _leftEncoderValues.ToArray();
if (encoderValues.Length > 1) // encoder values should be arriving at 5Hz.
{
double distanceDriven = Math.Abs(encoderValues[encoderValues.Length - 1] / 1000.0);
if (distanceDriven > Math.Abs(0.99 * distance) || Math.Abs(distanceDriven - Math.Abs(distance)) < 0.1)
{
LogMessage($"Completed drive with distance of {_leftEncoderValue / 1000.0:f3} meters.");
driving = false;
}
else
{
// THE FOLLOWING CODE AROUND HAZARDS IS CURRENTLY INACTIVE AS THE HAZARD SYSTEM IS DISABLED BY THE SKILL
// Not there yet. Everything progressing okay?
if (_stopHazardLatching)
{
// We've stopped due to a hazard. Wait a bit for it to go away.
LogMessage("Drive command paused for hazard.");
_misty.PlayAudio("s_Anger.wav", 100, OnResponse);
int maxHazardWait = 0;
while (_stopHazardState && maxHazardWait++ < 30)
{
if (_abort)
{
return(false);
}
await Task.Delay(1000);
}
if (_stopHazardState)
{
// Still in hazard state. Give up.
LogMessage("Giving up on drive command due to persistent hazard condition.");
success = false;
driving = false;
}
else
{
// We're out of hazard state.
LogMessage("Out of hazard state.");
distance -= encoderValues[encoderValues.Length - 1] / 1000.0;
_stopHazardLatching = false;
sendCommand = true;
}
}
if (encoderValues[encoderValues.Length - 1] == 0)
{
// Encoder values are not changing. Try sending drive command again.
if (retries++ > 5)
{
// Don't try forever.
LogMessage("Unable to complete drive command successfully.");
success = false;
driving = false;
}
else
{
sendCommand = true;
}
}
// Don't wait forever.
if (DateTime.Now.Subtract(start).TotalMilliseconds > (5 + 2 * duration))
{
LogMessage($"Time out waiting for completion of drive. Completed distance of {_leftEncoderValue / 1000.0:f3} meters.");
success = false;
driving = false;
}
}
}
else
{
LogMessage($"Not receiving encoder messages. Last encoder value recieved at {_lastEncoderValue}.");
success = false;
}
}
}
catch (Exception ex)
{
LogMessage("Exception occurred within DriveAsync: " + ex.Message);
success = false;
}
finally
{
_leftEncoderValues = null;
}
return(success);
}
