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