public async Task StartLogic()
{
while (true)
{
await Task.Delay(LOOP_WAIT_TIME);
//
if (_stopped)
{
Debug.WriteLine("Observer_stopped");
continue;
}
else
{
try
{
// Read filtered sensor values
foreach (var sensor in _mutualData.ReadFilteredData())
{
Debug.WriteLine(sensor.Key);
Debug.WriteLine(sensor.Value);
// Check if sensor value (distance from wall) is too small
if (sensor.Value < SAFETY_DISTANCE)
{
Debug.WriteLine("EMERGENCY STATE");
// MainLogic will see this
IsEmergencyStopEncountered = true;
for (int i = 0; i < 10; i++)
{
try
{
// fast motor stop
await _robot.setMotorSpeed(0, 0);
}
catch (Exception)
{
// Catch random exceptions
}
await Task.Delay(100);
}
break;
}
}
}
// Handle possible empty mutualData values
catch (NullReferenceException)
{
Debug.WriteLine("Encountered NullReferenceException at observer");
}
}
}
}
public async Task StartLogic()
{
while (true)
{
// await Task.Delay(LOOP_WAIT_TIME);
try
{
//
if (_stopped)
{
Debug.WriteLine("Stopped");
await Task.Delay(200);
}
// Run logic
else
{
Debug.WriteLine("Running");
// Robot started
if (RunMode == Enums.RobotRunMode.Idle)
{
Debug.WriteLine("Run mode: Idle");
// Wait a moment to acquire enough reliable sensor data
await Task.Delay(1500);
// Read filtered sensor values
var sensorValues = _mutualData.ReadFilteredData();
if (IsSensorValueInReach(sensorValues[Enums.Sensor.RightSensor]))
{
RunMode = Enums.RobotRunMode.FollowWall;
}
else
{
RunMode = Enums.RobotRunMode.FindWall;
}
}
// Robot doesn't see a wall, run forward till it does.
// If wall is seen on the side sensor closer, turn to that direction
else if (RunMode == Enums.RobotRunMode.FindWall)
{
Debug.WriteLine("Run mode: Find wall");
// Read filtered sensor values
var sensorValues = _mutualData.ReadFilteredData();
// check if other sensors find walls
if (IsSensorValueInReach(sensorValues[Enums.Sensor.LeftSensor]))
{
ControlMotors(0, 100);
await Task.Delay(2000); // Turn 40
}
else if (IsSensorValueInReach(sensorValues[Enums.Sensor.RearSensor]))
{
ControlMotors(0, 100);
await Task.Delay(6000); // Turn 180
}
while (!_stopped && sensorValues[Enums.Sensor.RightSensor] > 50)
{
sensorValues = _mutualData.ReadFilteredData();
if (sensorValues[Enums.Sensor.FrontSensor] < 70)
{
ControlMotors(0, 100);
await Task.Delay(1000);
}
else if (sensorValues[Enums.Sensor.LeftSensor] < 70)
{
ControlMotors(0, 100);
await Task.Delay(1500);
}
else
{
ControlMotors(100, 100);
await Task.Delay(100);
}
}
RunMode = Enums.RobotRunMode.FollowWall;
}
// Continues straight, till wall makes turn. Tries to follow it smoothly
else if (RunMode == Enums.RobotRunMode.FollowWall)
{
Debug.WriteLine("Run mode: Follow wall");
int margin = 8;
bool tightTurnDone = false;
while (!_stopped)
{
var sensorValues = _mutualData.ReadFilteredData();
double rightSensor = sensorValues[Enums.Sensor.RightSensor];
// Front sensor sees new wall
if (IsSensorValueInReach(sensorValues[Enums.Sensor.FrontSensor]))
{
// Turn left until robot is parallel with new wall
while (sensorValues[Enums.Sensor.FrontSensor] < 50 && !_stopped)
{
ControlMotors(0, 100);
await Task.Delay((int)(LOOP_WAIT_TIME));
sensorValues = _mutualData.ReadFilteredData();
}
}
// Continue straight if right sensor reading is within margin
if (rightSensor > IDEAL_RIGHT_SENSOR_DISTANCE - margin &&
rightSensor < IDEAL_RIGHT_SENSOR_DISTANCE + margin)
{
ControlMotors(100, 100);
await Task.Delay(LOOP_WAIT_TIME);
}
// If wall ends, continue for a while
else if (!IsSensorValueInReach(rightSensor) && !tightTurnDone)
{
ControlMotors(100, 100);
tightTurnDone = true;
await Task.Delay(350);
}
// "P-controller", difference from right sensor value and setpoint.
// Controls with kind of a pulse-width modulation by stopping track
// Larger difference causes longer stopping sequence
else
{
MakeRoughOrientationCorrection(rightSensor);
double i = Math.Abs(rightSensor - IDEAL_RIGHT_SENSOR_DISTANCE) / 4;
i = Math.Min(i, 10);
await Task.Delay((int)(LOOP_WAIT_TIME *i));
// continue forward for a while to get new sensor data
ControlMotors(100, 100);
await Task.Delay(LOOP_WAIT_TIME *3);
tightTurnDone = false;
}
}
}
}
}
catch (Exception)
{
// Random exception
}
}
}