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);
}
public async Task <bool> MoveToAsync(double x, double y, double yaw, double tolerance)
{
if (!_tracking)
{
return(false);
}
// Get bearing to the destination
double bearing = 0;
if (_mapCell.X > x)
{
if (_mapCell.Y > y)
{
bearing = -90 - 180 * Math.Atan((_mapCell.X - x) / (_mapCell.Y - y)) / Math.PI;
}
else
{
bearing = 90 + 180 * Math.Atan((_mapCell.X - x) / (y - _mapCell.Y)) / Math.PI;
}
}
else
{
bearing = 180 * Math.Atan((y - _mapCell.Y) / (x - _mapCell.X)) / Math.PI;
}
_skillHelper.LogMessage($"Bearing to destination is {bearing:f2}.");
// Turn towards destination
await _skillHelper.TurnAsync(bearing - _mapYaw);
if (_abort)
{
return(false);
}
_skillHelper.LogMessage($"Map cell is [{_mapCell.X},{_mapCell.Y}]. Map yaw is {_mapYaw:f2}.");
// Get distance to destination and drive there.
// Note: assuming constant 0.04 meters per cell.
double distance = 0.04 * Math.Sqrt(Math.Pow(_mapCell.X - x, 2) + Math.Pow(_mapCell.Y - y, 2));
await _skillHelper.DriveAsync(distance);
if (_abort)
{
return(false);
}
_skillHelper.LogMessage($"Map cell is [{_mapCell.X},{_mapCell.Y}]. Map yaw is {_mapYaw:f2}.");
// Turn to desired yaw
double delta = yaw - _mapYaw;
while (Math.Abs(delta) > 1)
{
// Can't turn less than 3 degrees.
if (Math.Abs(delta) < 3)
{
if (delta < 0)
{
delta = -3;
}
else
{
delta = 3;
}
}
await _skillHelper.TurnAsync(delta);
if (_abort)
{
return(false);
}
await Task.Delay(500);
delta = yaw - _mapYaw;
_skillHelper.LogMessage($"Map cell is [{_mapCell.X},{_mapCell.Y}]. Map yaw is {_mapYaw:f2}.");
}
delta = y - _mapCell.Y;
while (Math.Abs(delta) > tolerance)
{
await _skillHelper.TurnAsync(90);
await _skillHelper.DriveAsync(0.04 *delta);
await _skillHelper.TurnAsync(-90);
_skillHelper.LogMessage($"Map cell is [{_mapCell.X},{_mapCell.Y}]. Map yaw is {_mapYaw:f2}.");
delta = y - _mapCell.Y;
if (_abort)
{
return(false);
}
}
return(true);
}
private Task <bool> DriveAsync(double distance)
{
return(_skillHelper.DriveAsync(distance));
}