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);
}
public async Task <bool> AlignToArTag(int dictionary, double size, int tagId, Simple2DPose goalTagPose)
{
_abort = false;
_tagId = tagId;
_skillHelper.LogMessage($"Starting AR tag alignment. Desired tag pose is [x, y, yaw] {goalTagPose.X:f3}, {goalTagPose.Y:f3}, {goalTagPose.Yaw.Degrees():f0}].");
// Look straight ahead
await _skillHelper.MoveHeadAsync(0, 0, 0);
// Startup AR tag detection
_misty.EnableCameraService(OnResponse);
_misty.EnableCameraService(OnResponse); // Calling twice to make sure. TODO: something smarter
_misty.StartArTagDetector(dictionary, size, OnResponse);
_misty.StartArTagDetector(dictionary, size, OnResponse); // Calling twice to make sure. TODO: something smarter
_misty.RegisterArTagDetectionEvent(ArTagCallback, 100, true, "artagevent", OnResponse);
// Wait for measurments to start.
if (!await WaitForMeasurementAsync(10, 2))
{
if (_abort)
{
return(false);
}
// Never detected the AR tag. Try a small sweep.
_skillHelper.MistySpeak("I can't see the tag. Looking around for it.");
_skillHelper.LogMessage("Do not see the tag. Starting sweep.");
bool arTagFound = false;
await _skillHelper.TurnAsync(20);
int maxTurns = 5;
while (!arTagFound && --maxTurns >= 0)
{
arTagFound = await WaitForMeasurementAsync(1, 1);
if (_abort)
{
return(false);
}
if (!arTagFound)
{
await _skillHelper.TurnAsync(-10);
}
}
if (!arTagFound)
{
_skillHelper.MistySpeak("I cannot find the tag.");
_skillHelper.LogMessage("Never detected the AR tag.");
Cleanup();
return(false);
}
}
_skillHelper.LogMessage($"AR tag located at [x, y, yaw] : [{_currentTagPose.X:f3}, {_currentTagPose.Y:f3}, {_currentTagPose.Yaw.Degrees():f1}]. " +
$"Offset : [{(_currentTagPose.X - goalTagPose.X):f3}, {(_currentTagPose.Y - goalTagPose.Y):f3}, {(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()):f1}].");
if (Math.Abs(_currentTagPose.X - goalTagPose.X) < X_TOLERANCE && Math.Abs(_currentTagPose.Y - goalTagPose.Y) < Y_TOLERANCE &&
Math.Abs(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()) < YAW_TOLERANCE)
{
_skillHelper.MistySpeak("We're already aligned with the tag. No need to move.");
_skillHelper.LogMessage("No movement needed to align to tag.");
}
else
{
int retries = 0;
while (Math.Abs(_currentTagPose.X - goalTagPose.X) > X_TOLERANCE || Math.Abs(_currentTagPose.Y - goalTagPose.Y) > Y_TOLERANCE ||
Math.Abs(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()) > YAW_TOLERANCE)
{
MoveSequence moveSequence = NavigationHelper.CalculateMoveSequence(_currentTagPose, goalTagPose);
_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.");
await _skillHelper.TurnAsync(moveSequence.Turn1.Degrees());
if (_abort)
{
return(false);
}
await _skillHelper.DriveAsync(moveSequence.DriveDistance, true);
if (_abort)
{
return(false);
}
await _skillHelper.TurnAsync(moveSequence.Turn2.Degrees());
if (_abort)
{
return(false);
}
_skillHelper.LogMessage($"AR tag located at [x, y, yaw] : [{_currentTagPose.X:f3}, {_currentTagPose.Y:f3}, {_currentTagPose.Yaw.Degrees():f1}]. " +
$"Offset : [{(_currentTagPose.X - goalTagPose.X):f3}, {(_currentTagPose.Y - goalTagPose.Y):f3}, {(_currentTagPose.Yaw.Degrees() - goalTagPose.Yaw.Degrees()):f1}].");
if (retries++ > 5)
{
_skillHelper.LogMessage("Failed to line up with the tag.");
_skillHelper.MistySpeak("I can't seem to line up right. I give up.");
await Task.Delay(5000);
Cleanup();
return(false);
}
}
_skillHelper.MistySpeak("Alignment complete.");
}
Cleanup();
return(true);
}
