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)
{
ThrowExceptionOnSensorError();
if (_ultrasonic.Fwd < 0.5)
{
_wheels.Stop();
Thread.Sleep(500);
RotateAwayFromObstruction(cancellationToken, desiredFrontalClearance: 1.0f, rotationSpeedPercentage: 50);
}
else
{
float reductionRate = 1.0f;
KeepCenterCorridor(reductionRate);
}
}
public override void Run(CancellationToken cancellationToken)
{
ThrowExceptionIfCollectorIsStopped();
const int rotateLimit = 60;
float angleToLargestDistance = _lidar.LargestDistance.Angle;
if (float.IsNaN(angleToLargestDistance))
{
_wheels.Stop();
Debug.WriteLine("STOPPED WHEELS due to no LIDAR distance found!", "ControlLogic");
Thread.Sleep(200);
}
else if (angleToLargestDistance > rotateLimit && angleToLargestDistance < 360 - rotateLimit)
{
TurnTowardsLargestDistance(cancellationToken);
}
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);
}