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; }