/// <summary>
/// Rotates away from obstruction
/// </summary>
/// <param name="cancellationToken">Used to exit loop if control logic stop is requested</param>
/// <param name="desiredFrontalClearance">Distance required in front for it to stop rotating (in meters)</param>
/// <param name="rotationSpeedPercentage">Value between -100 and +100</param>
private void RotateAwayFromObstruction(CancellationToken cancellationToken, float desiredFrontalClearance, int rotationSpeedPercentage)
{
DateTime startedRotationAt = DateTime.Now;
bool rightHasLargestDistance = _ultrasonic.Right > _ultrasonic.Left;
while (_ultrasonic.Fwd < desiredFrontalClearance && !cancellationToken.IsCancellationRequested && !UnacknowledgedSensorError())
{
if (rightHasLargestDistance)
{
_wheels.TurnRight(rotationSpeedPercentage);
}
else
{
_wheels.TurnLeft(rotationSpeedPercentage);
}
Thread.Sleep(50);
if (startedRotationAt - DateTime.Now > TimeSpan.FromSeconds(5))
{
_wheels.Stop();
Thread.Sleep(2000);
}
}
_wheels.Stop();
}
public void TurnRight(int degrees)
{
_frontLeft.TurnRight(degrees);
_frontRight.TurnRight(degrees);
_rearLeft.TurnRight(degrees);
_rearRight.TurnRight(degrees);
}
private void RotateTowardsLargestDistance(float angle, int turnSpeed)
{
if (angle < 180)
{
_wheels.TurnRight(turnSpeed);
}
else
{
_wheels.TurnLeft(turnSpeed);
}
}
private void EmergencySteerFromObstacleInFront(float desiredClearance, int rotationPowerPercentage, CancellationToken cancellationToken)
{
DateTime startedRotationAt = DateTime.Now;
bool rightHasLargestDistance = _lidar.LargestDistanceInRange(260, 100).Angle < 180;
while (_ultrasonic.Fwd < desiredClearance && !cancellationToken.IsCancellationRequested)
{
if (rightHasLargestDistance)
{
_wheels.TurnRight(rotationPowerPercentage);
}
else
{
_wheels.TurnLeft(rotationPowerPercentage);
}
Thread.Sleep(50);
if (DateTime.Now - startedRotationAt > TimeSpan.FromSeconds(5))
{
Debug.WriteLine($"Inside started rotation. where rotationPowerPrecentage is {rotationPowerPercentage}");
if (rotationPowerPercentage < 100)
{
rotationPowerPercentage += 10;
Debug.WriteLine($"Increased power by 10. Is now {rotationPowerPercentage}");
}
else
{
_wheels.Stop();
Thread.Sleep(5000);
}
startedRotationAt = DateTime.Now;
}
}
_wheels.Stop();
}
public void TurnRight(int degrees)
{
_frontCentre.TurnRight(degrees);
}
