public Controller()
{
Roomba = new Roomba();
Lidar = new Lidar();
Navigation = new Navigation();
Analysis = new LidarAnalysis();
}
public void Dispose()
{
Roomba.Close();
Lidar.Close();
IsEnabled = false;
}
/// <summary>
/// This function will move the robot while also stopping if there are obstacles and adjusting
/// course if it's heading towards a wall
/// </summary>
/// <param name="speed">Speed robot will move</param>
/// <param name="distMM">Distance in millimeters</param>
/// <returns></returns>
public async Task ContinuousMove(int speed, int distMM, CancellationToken token)
{
if (!Roomba.IsEnabled)
{
Debug.WriteLine("Roomba is not enabled! Cancelling move...");
return;
}
lastStopType = StopReason.Success;
int distMoved = 0;
do
{
// Cancel the entire move if we collided with something or canceled everything
if (token != null && token.IsCancellationRequested)
{
return;
}
if (lastStopType == StopReason.Collision || lastStopType == StopReason.Cancellation)
{
return;
}
switch (Analysis.LastRecommendation)
{
case LidarRecommendation.Move:
Debug.WriteLine("ContinuousMove: Moving...");
// Calculate distance left to move
distMM -= distMoved;
// Move the robot
OnControllerMessageReceived($"Moving {distMM} mm...");
await Roomba.MoveDistanceEncoders(speed, distMM);
// Determine how far the robot moved
distMoved = (int)Roomba.AverageDistanceTraveled;
break;
case LidarRecommendation.AdjustObstacle:
Debug.WriteLine("ContinuousMove: Adjusting...");
// It doesn't matter what the actual adjust angle is, since the robot will
// stop rotating once we get the recommendation to move
if (Analysis.LastAdjustAngle < 0)
{
Roomba.SetSpeed(DEFAULT_ROTATE_SPEED, 0);
}
else
{
Roomba.SetSpeed(0, DEFAULT_ROTATE_SPEED);
}
break;
}
await Task.Delay(100);
}while (distMoved < distMM);
}
private async Task ExecuteNavigationInstruction(Instruction instruction)
{
Debug.WriteLine($"Executing nav instruction -> {instruction}");
OnControllerMessageReceived($"Executing navigation instruction -> {instruction}");
if (instruction.Type == InstructionType.Angle)
{
await Roomba.RotateEncoders(DEFAULT_ROTATE_SPEED, instruction.Data);
}
else if (instruction.Type == InstructionType.Distance)
{
await ContinuousMove(DEFAULT_MOVE_SPEED, (int)(1000 * instruction.Data), cts.Token);
}
}
/// <summary>
/// Event handler for when LIDAR analysis comes up with new recommendation
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void Analysis_AnalysisChanged(object sender, AnalysisEventArgs e)
{
Debug.WriteLine("Analysis_AnalysisChanged: " + Enum.GetName(typeof(LidarRecommendation), e.Recommendation));
switch (e.Recommendation)
{
case LidarRecommendation.Stop:
Roomba.Halt();
break;
case LidarRecommendation.AdjustObstacle:
Debug.WriteLine("Analysis_AnalysisChanged - Adjust Angle: " + e.AdjustAngle);
Roomba.CancelMove();
break;
case LidarRecommendation.Move:
break;
}
}
/// <summary>
/// Initializes the Roomba and LIDAR
/// </summary>
/// <param name="roombaPortId">Serial device ID for the Roomba</param>
/// <param name="lidarPortId">Serial device ID for the LIDAR</param>
/// <returns></returns>
public async Task InitializeAsync(string roombaDeviceId = Roomba.DEFAULT_DEVICE_ID, string lidarDeviceId = Lidar.DEFAULT_DEVICE_ID)
{
Debug.WriteLine("Initializing Controller...");
OnControllerMessageReceived("Initializing controller...");
// Initialize the Roomba
await Roomba.InitializeAsync(roombaDeviceId, 20);
Roomba.Stopped += Roomba_Stopped;
OnControllerMessageReceived("Roomba successfully initialized.");
// Initialize the LIDAR
await Lidar.InitializeAsync(lidarDeviceId);
Lidar.ScanComplete += Lidar_ScanComplete;
Lidar.StartScan();
OnControllerMessageReceived("LIDAR successfully initialized.");
Analysis.AnalysisChanged += Analysis_AnalysisChanged;
Debug.WriteLine("Controller initialization completed.");
IsEnabled = true;
}
