private void ChargerPoseMessage(IChargerPoseEvent eventResponse)
{
// Using averaged value of charger pose.
if (_poseLock)
{
return;
}
_poseLock = true;
try
{
if (_previousChargerPose.Count > 5)
{
_previousChargerPose.Dequeue();
}
var latestPose = new ChargerPosition()
{
X = eventResponse.Pose.HomogeneousMatrix[12],
Y = eventResponse.Pose.HomogeneousMatrix[13],
Z = eventResponse.Pose.HomogeneousMatrix[14],
//EulerYaw = (float)(180.0 * Math.Atan2(eventResponse.Pose.HomogeneousMatrix[6], eventResponse.Pose.HomogeneousMatrix[2]) / Math.PI)
EulerYaw = (float)(-180.0 * Math.Asin(eventResponse.Pose.HomogeneousMatrix[2]) / Math.PI)
};
_previousChargerPose.Enqueue(latestPose);
var meanPose = new ChargerPosition();
foreach (var pose in _previousChargerPose)
{
meanPose.X += pose.X;
meanPose.Y += pose.Y;
meanPose.Z += pose.Z;
meanPose.EulerYaw += pose.EulerYaw;
}
meanPose.X /= _previousChargerPose.Count;
meanPose.Y /= _previousChargerPose.Count;
meanPose.Z /= _previousChargerPose.Count;
meanPose.EulerYaw /= _previousChargerPose.Count;
//_skillHelper.LogMessage($"{latestPose.X} {meanPose.X}");
// Apply offset so that we can treat charger position as relative to the center
// of the Occipital sensor.
meanPose.X += CENTER_OFFSET;
//_skillHelper.LogMessage($"Charger pose: X {meanPose.X} Y {meanPose.Y} Z {meanPose.Z} EY {meanPose.EulerYaw}");
_chargerPose = meanPose;
}
finally
{
_poseLock = false;
}
}
private async Task <bool> FindChargerAsync()
{
try
{
_skillHelper.LogMessage("Initiating search for charger.");
// Clear pose and pause to see if we can already see the charger.
_chargerPose = null;
await Task.Delay(3000);
if (_abort)
{
return(false);
}
if (_chargerPose != null)
{
_skillHelper.LogMessage($"Charger at [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Z:f3}] meters. Euler yaw is {_chargerPose.EulerYaw:f3} degrees.");
}
else
{
// Perform a sweep to look for the charger.
// This is based upon the assumption that we just drove back to the charger and are roughly facing it.
await _skillHelper.TurnAsync(INITIAL_ROTATION_BEFORE_SWEEP);
_chargerPose = null;
if (_abort)
{
return(false);
}
if (!await SweepForCharger())
{
// Did not find charger after a full spin. Check our TOF distance assuming that charger is against a wall.
await _skillHelper.TurnAsync(AxisRotation(_skillHelper.ImuYaw, _initialYaw));
if (_abort)
{
return(false);
}
TofValues tofValues = _skillHelper.GetTofValues();
if (_abort)
{
return(false);
}
double?distance = tofValues.FrontCenter.HasValue ? tofValues.FrontCenter.Value : tofValues.FrontLeft.HasValue ? tofValues.FrontLeft.Value : tofValues.FrontRight.Value;
if (distance.HasValue)
{
// Drive to about 1 meter from wall
await _skillHelper.DriveAsync(distance.Value - 1.0);
// Sweep again.
await _skillHelper.TurnAsync(INITIAL_ROTATION_BEFORE_SWEEP);
if (_abort)
{
return(false);
}
if (!await SweepForCharger())
{
_skillHelper.LogMessage("Never found the charger.");
return(false);
}
}
}
}
_skillHelper.LogMessage($"Charger at [{_chargerPose.X:f3}, {_chargerPose.Y:f3}, {_chargerPose.Z:f3}] meters. Euler yaw is {_chargerPose.EulerYaw:f3} degrees.");
}
catch (Exception ex)
{
_skillHelper.LogMessage("An exception occurred within ChargerDock.FindChargerAsync: " + ex.Message);
}
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);
}
