public void TurnLeftUntilWall(SensorManager sensors)
{
brake();
//MoveForward(100, GlobalVal.speed);
turnLeft(45);
while (sensors.getDistance(Sensor.Central) > GlobalVal.distanceToDetect)
{
float wall = sensors.getDistance(Sensor.Wall);
float wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
{
break;
}
else
{
if (wall < wall_back)
{
turnRight(5);
MoveForward(10, GlobalVal.speed);
continue;
}
else
{
turnLeft(10);
MoveForward(10, GlobalVal.speed);
continue;
}
}
}
}
public void MoveToObject(SensorManager sensors)
{
this.resetDistance();
while (sensors.getDistance(Sensor.Central) > GlobalVal.distanceToDetect && !contingency.alarm)
{
MoveForward(50, GlobalVal.speed);
}
this.brake();
}
public void TurnRightUntilWall(SensorManager sensors)
{
Pid = false;
float central, wall, wall_back;
//double distancia_pared;
K = 10;
if (Pid == true)
{
central = sensors.getDistance(Sensor.Central);
turnRight((int)(central / (central - GlobalVal.minDistanceToFollowWall)));
while (!contingency.alarm)
{
wall = sensors.getDistance(Sensor.Wall);
wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
{
break;
}
else
{
central = sensors.getDistance(Sensor.Central);
//distancia_pared = (exMath.cos(Relative_MAG_Heading)) * (central);
//turnRight((int)(distancia_pared / (distancia_pared - GlobalVal.minDistanceToFollowWall)) * K);
}
MoveForward(5);
}
}
if (Pid == false)
{
brake();
int s = 55;
turnRight(s);
Debug.Print("angle_Def:" + s.ToString());
//Thread.Sleep(600);
while (!contingency.alarm)
{
wall = sensors.getDistance(Sensor.Wall);
wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
{
break;
}
else
{
turnRight(5);
}
}
}
}
public void TurnRightUntilWall(SensorManager sensors)
{
Pid = false;
float central, wall, wall_back;
//double distancia_pared;
K = 10;
if (Pid == true)
{
central = sensors.getDistance(Sensor.Central);
turnRight((int)(central / (central - GlobalVal.minDistanceToFollowWall)));
while (!contingency.alarm)
{
wall = sensors.getDistance(Sensor.Wall);
wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
break;
else
{
central = sensors.getDistance(Sensor.Central);
//distancia_pared = (exMath.cos(Relative_MAG_Heading)) * (central);
//turnRight((int)(distancia_pared / (distancia_pared - GlobalVal.minDistanceToFollowWall)) * K);
}
MoveForward(5);
}
}
if (Pid == false)
{
brake();
int s=55;
turnRight(s);
Debug.Print("angle_Def:" + s.ToString());
//Thread.Sleep(600);
while (!contingency.alarm)
{
wall = sensors.getDistance(Sensor.Wall);
wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis)
break;
else
turnRight(5);
}
}
}
public void TurnLeftUntilWall(SensorManager sensors)
{
brake();
//MoveForward(100, GlobalVal.speed);
turnLeft(45);
while (sensors.getDistance(Sensor.Central) > GlobalVal.distanceToDetect)
{
float wall = sensors.getDistance(Sensor.Wall);
float wall_back = sensors.getDistance(Sensor.wall_back);
if (exMath.Abs(wall - wall_back) <= GlobalVal.hysteresis) { break; }
else
{
if (wall < wall_back)
{
turnRight(5);
MoveForward(10, GlobalVal.speed);
continue;
}
else
{
turnLeft(10);
MoveForward(10, GlobalVal.speed);
continue;
}
}
}
}
public void MoveToObject(SensorManager sensors)
{
this.resetDistance();
while (sensors.getDistance(Sensor.Central) > GlobalVal.distanceToDetect && !contingency.alarm) { MoveForward(50, GlobalVal.speed); }
this.brake();
}
void sensorTimer_tick(Object state)
{
short l1 = (short)(sensors.getDistance(Sensor.Central) * 10);
short s2 = (short)(sensors.getDistance(Sensor.Wall) * 10);
short s1 = (short)(sensors.getDistance(Sensor.wall_back) * 10);
short l2 = (short)(sensors.getDistance(Sensor.Right) * 10);
lock (coder)
{
short value;
if (s1 > 300)
{
value = -1;
}
else if (s1 < 40)
{
value = -2;
}
else
{
value = s1;
}
coder.Send(Message.SensorS1, value);
;
if (s2 > 300)
{
value = -1;
}
else if (s2 < 40)
{
value = -2;
}
else
{
value = s2;
}
coder.Send(Message.SensorS2, value);
if (l1 > 800)
{
value = -1;
}
else if (l1 < 100)
{
value = -2;
}
else
{
value = l1;
}
coder.Send(Message.SensorL1, value);
if (l2 > 800)
{
value = -1;
}
else if (l2 < 100)
{
value = -2;
}
else
{
value = l2;
}
coder.Send(Message.SensorL2, value);
}
}