/// <summary>
/// Loads a sequence of Drive and Turn commands from a text file within the \Data\Users\DefaultAccount\Music\ folder
/// </summary>
public async Task <List <IFollowPathCommand> > LoadCommandsAsync(string filename, Func <Task <bool> > delegateCommand)
{
var commands = new List <IFollowPathCommand>();
try
{
var folder = Windows.Storage.KnownFolders.MusicLibrary;
var file = await folder.GetFileAsync(filename);
var lines = await Windows.Storage.FileIO.ReadLinesAsync(file);
foreach (string line in lines)
{
string[] split = line.Split(':');
string cmd = split[0].ToUpper();
double.TryParse(split[1], out double arg);
switch (cmd)
{
case "DRIVE":
commands.Add(new DriveCommand(arg));
break;
case "TURN":
commands.Add(new TurnCommand(arg));
break;
case "DELEGATE":
commands.Add(new DelegateCommand(delegateCommand));
break;
}
}
}
catch (Exception ex)
{
_skillHelper.LogMessage($"Failed to read commands file, {filename}: {ex.Message}.");
}
return(commands);
}
public async Task <bool> StartTrackingAsync(string mapName)
{
_skillHelper.LogMessage($"Attempting to track within map {mapName}.");
_abort = false;
// We don't get the newly loaded map back from Occ software until we start and stop tracking. So...
_skillHelper.LogMessage("Loading map and starting tracking.");
_misty.SetCurrentSlamMap(mapName, OnResponse);
await Task.Delay(1000);
_misty.StartTracking(OnResponse);
await _skillHelper.MoveHeadAsync(_headPitchOffset, _headRollOffset, _headYawOffset);
_misty.RegisterSlamStatusEvent(SlamStatusCallback, 0, true, "MapDockSlamStatusEvent", null, OnResponse);
_misty.RegisterSelfStateEvent(SelfStateCallback, 250, true, "MapDockSelfStateEvent", OnResponse);
// There is a current defect where switching maps does not fully take effect until we start tracking
// and stop tracking. So we need to start, stop, and re-start.
_misty.StopTracking(OnResponse);
await Task.Delay(4000);
_misty.StartTracking(OnResponse);
await Task.Delay(4000);
_misty.GetMap(OnResponse);
await Task.Delay(4000);
if (_slamStatus == null || _slamStatus.SensorStatus != MistyRobotics.Common.Types.SlamSensorMode.Streaming)
{
_skillHelper.LogMessage("Failed to start tracking.");
Cleanup();
return(false);
}
_skillHelper.LogMessage("Checking for pose.");
int count = 0;
while (_slamStatus.RunMode != MistyRobotics.Common.Types.SlamRunningMode.Tracking && count++ < 40)
{
await _skillHelper.TurnAsync(10);
_misty.GetSlamStatus(OnResponse);
}
if (_slamStatus.RunMode == MistyRobotics.Common.Types.SlamRunningMode.Tracking)
{
_skillHelper.LogMessage($"Pose acquired. Map cell is [{_mapCell.X},{_mapCell.Y}]. Map yaw is {_mapYaw:f2}.");
_tracking = true;
}
else
{
_skillHelper.LogMessage("Unable to obtain pose.");
Cleanup();
_tracking = false;
}
return(_tracking);
}
public async Task <bool> DockAsync()
{
_skillHelper.LogMessage("Execute charger docking.");
_misty.RegisterBatteryChargeEvent(BatteryChargingMessage, 250, true, null, "BatteryChargingEvent", OnResponse);
_previousChargerPose = new Queue <ChargerPosition>();
_abort = false;
// Pitch sometimes has an offset even when Misty is level.
_initialPitch = AxisLimited(_skillHelper.ImuPitch);
_skillHelper.LogMessage($"Initial pitch is {_initialPitch:f3}.");
_initialYaw = _skillHelper.ImuYaw;
_misty.RegisterChargerPoseEvent(ChargerPoseMessage, 100, true, "ChargerPoseEvent", OnResponse);
if (!await StartDockDetectorAsync())
{
Cleanup();
return(false);
}
// Try to dock N times.
bool docked = false;
int retries = 0;
while (!docked && retries++ < DOCKING_MAX_RETRIES)
{
docked = await ExecuteDockAsync();
}
Cleanup();
return(docked);
}
