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); } } }
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); } }