public override void Run(CancellationToken cancellationToken) { ThrowExceptionIfLidarCollectorIsStoppedOrSensorError(); float angleToLargestDistance = _lidar.LargestDistanceInRange(260, 100).Angle; if (float.IsNaN(angleToLargestDistance)) { _wheels.Stop(); Debug.WriteLine("STOPPED due to no LIDAR distance found in range!", "ControlLogic"); Thread.Sleep(200); } else { _speedLeft = BaseSpeed; _speedRight = BaseSpeed; CompensateSpeedTowardsAngle(angleToLargestDistance); CompensateSpeedFromSideClearance(); if (_ultrasonic.Fwd < clearanceLimitFwd) { Debug.WriteLine("Emergency steer Fwd started.", "ControlLogic"); EmergencySteerFromObstacleInFront(1.3f, 50, cancellationToken); Debug.WriteLine("Emergency steer Fwd completed.", "ControlLogic"); } else { _wheels.SetSpeed(_speedLeft, _speedRight); } } Thread.Sleep(50); }
public override void Run(CancellationToken cancellationToken) { ThrowExceptionIfLidarCollectorIsStoppedOrSensorError(); float angleToLargestDistance = _lidar.LargestDistanceInRange(260, 100).Angle; if (float.IsNaN(angleToLargestDistance)) { _wheels.Stop(); Debug.WriteLine("STOPPED WHEELS due to no LIDAR distance found in range!", "ControlLogic"); Thread.Sleep(200); } else { SteerTowardsAngle(angleToLargestDistance, 100); } Thread.Sleep(50); }