public void Act()
{
while (!end)
{
if (alarm)
{
lock (monitor)
{
if (count >= 5)
{
Engine.getInstance().Debug("Contingency count: " + count);
if (count >= 10)
{
navigation.turnLeft(20, (sbyte)90);
}
else if (count >= 15)
{
navigation.turnRight(20, (sbyte)90);
count = 10;
}
else
{
navigation.MoveForward(100, (sbyte)90);
}
}
}
Engine.getInstance().Debug("Solving problem when moving " + movement.ToString());
switch (movement)
{
case Movement.forward:
navigation.MoveBackward(100, (sbyte)40);
break;
case Movement.backward:
navigation.MoveForward(100, (sbyte)40);
break;
case Movement.left:
navigation.turnRight(10, (sbyte)40);
break;
case Movement.right:
navigation.turnLeft(10, (sbyte)50);
break;
}
Engine.getInstance().Restart();
alarm = false;
}
}
}
private void _Run()
{
while (!cancel)
{
debug.Write(true);
Thread.Sleep(50);
debug.Write(false);
if (currentMode == Mode.SearchingForWall)
{
navigation.MoveToObject(sensors);
float central = sensors.getDistance(Sensor.Central);
if (central <= GlobalVal.distanceToDetect)
{
//WALL FOUND
navigation.TurnRightUntilWall(sensors);
currentMode = Mode.FollowWall;
Info("Current mode: Follow Wall");
}
continue;
}
else if (currentMode == Mode.FollowWall)
{
float central;
float wall;
float wallback;
double error_distance, angle_error;
//FOLLOW LEFT WALL
while (!cancel)
{
wall = sensors.getDistance(Sensor.Wall);
wallback = sensors.getDistance(Sensor.wall_back);
central = sensors.getDistance(Sensor.Central);
error_distance = 0;
angle_error = 0;
if (central < GlobalVal.distanceToDetect)
{
break;
}
if (exMath.Abs(wall - wallback) < GlobalVal.hysteresis)
{
if (wall < 30 && wallback < 30)
{
//Maintain certain distance to the wall (too close may be a problem)
if (wall < GlobalVal.minDistanceToFollowWall || wallback < GlobalVal.minDistanceToFollowWall)
{
if (wall <= wallback)
{
navigation.turnRight(10);
}
}
navigation.MoveForward(20);
}
else
{
continue;
}
}//CORRECT THE DEVIATION RESPECT TO THE WALL
else if (wall < wallback)
{
error_distance = wall - wallback;
if (error_distance < 5)
{
angle_error = exMath.Atan2(error_distance * 10, GlobalVal.length_mm);
navigation.turnLeft((int)(angle_error / 2));
navigation.MoveForward(30);//50
}
else//THERE IS A CORNER
{
Debug("LEFT CORNER");
navigation.TurnLeftUntilWall(sensors);
}
}//IN THE FOLLOWING CASE:
//1-IS A WALL DEVIATION
//2-THERE IS A CORNER
else if (wall > wallback)
{
if ((wall - wallback) < 5)
{
navigation.turnLeft(1);
navigation.MoveForward(50);
}
else//THERE IS A CORNER
{
Debug("LEFT CORNER");
navigation.TurnLeftUntilWall(sensors);
}
}
}
if (!cancel)
{
navigation.TurnRightUntilWall(sensors);
}
}
}
}