public void ObstacleListener(OBSTACLE_DIRECTION direction, bool isThereAnObstacle)
{
if (!Map.MapInformation.isObstacleDetecteurOn())
{
Informations.printInformations(Priority.MEDIUM, "Mouvement - ObstacleListener - !Map.MapInformation.isObstacleDetecteurOn(), no change on trajectory. ");
return;
}
if (direction == Robot.robot.BASE_ROULANTE.GetDirection())
{
if (isThereAnObstacle /*&& Robot.robot.BASE_ROULANTE.kangaroo.currentMode == "D"*/)
{
Informations.printInformations(Priority.MEDIUM, "Mouvement - ObstacleListener - obstacle detected : pausing movement. ");
this.Pause();
}
if (!isThereAnObstacle)
{
Informations.printInformations(Priority.MEDIUM, "Mouvement - ObstacleListener - obstacle removed : resuming movement. ");
//Robot.robot.IHM.AfficherInformation("UNPAUSED", false);
//new Thread(() => this.Start()).Start();
isPaused = false;
}
}
else
{
Informations.printInformations(Priority.MEDIUM, "Mouvement - ObstacleListener - obstacle in other direction : ignored. ");
}
}
public Mouvement(PointOriente destination, OBSTACLE_DIRECTION forcedDirection, int speedDrive)
{
this.destination = destination;
this.isDirectionForced = true;
this.forcedDirection = forcedDirection;
this.speedDrive = speedDrive;
this.speedTurn = Robot.robot.BASE_ROULANTE.speedTurn;
}
public Mouvement(PointOriente destination, OBSTACLE_DIRECTION forcedDirection, int speedDrive, int speedTurn)
{
this.destination = destination;
this.isDirectionForced = true;
this.forcedDirection = forcedDirection;
this.speedDrive = speedDrive;
this.speedTurn = speedTurn;
}
public Mouvement(PointOriente destination, bool adjustToAngle, OBSTACLE_DIRECTION forcedDirection)
{
this.adjustToAngle = adjustToAngle;
this.destination = destination;
this.isDirectionForced = true;
this.forcedDirection = forcedDirection;
this.speedDrive = Robot.robot.BASE_ROULANTE.speedDrive;
this.speedTurn = Robot.robot.BASE_ROULANTE.speedTurn;
}
private bool getObstacleStatusForDirection(OBSTACLE_DIRECTION direction, bool[] obstaclesBySensor)
{
for (int i = 0; i < this.capteurs.Length; i++)
{
if (capteurs[i].direction == direction && obstaclesBySensor[i])
{
return(true);
}
}
return(false);
}
public Ultrason(int socket, OBSTACLE_DIRECTION direction)
: base(direction)
{
capteurUltrason = new GTM.GHIElectronics.DistanceUS3(socket);
capteurUltrason.DebugPrintEnabled = true;
//lastStatus = IsThereAnObstacle();
/*Gadgeteer.Timer timer = new Gadgeteer.Timer(REFRESH_RATE);
* timer.Tick += this.detectObstacle;
* timer.Start();*/
}
public Infrarouge(int socket, int port, OBSTACLE_DIRECTION direction)
: base(direction)
{
Informations.printInformations(Priority.MEDIUM, "New infrarouge on socket " + socket + " & port " + port + "; pin : " + Socket.GetSocket(socket, true, null, null).CpuPins[port] + "; type of pin : " + Socket.GetSocket(socket, true, null, null).CpuPins[port].GetType());
this.capteurIR = new InterruptPort(
Socket.GetSocket(socket, true, null, null).CpuPins[port],
true,
Port.ResistorMode.PullUp,
Port.InterruptMode.InterruptEdgeBoth);
this.capteurIR.OnInterrupt += (uint pin, uint state, DateTime time) => OnObstacleChange(state == 0);
}
public bool getObstacleStatusForDirection(OBSTACLE_DIRECTION direction)
{
return(getObstacleStatusForDirection(direction, this.lastObstacleFoundForCapteurs));
}
public CapteurObstacle(OBSTACLE_DIRECTION direction)
{
this.direction = direction;
}
public Boolean GoToOrientedPoint(PointOriente pt, OBSTACLE_DIRECTION forceDir) // forceDir = AVANT or ARRIERE
{
isPaused = Map.MapInformation.isDetecteurOn && Robot.robot.OBSTACLE_MANAGER.getObstacleStatusForDirection(forceDir);
if (isPaused)
{
return(false);
}
Informations.printInformations(Priority.HIGH, "Going (dir forced) to " + pt.x.ToString() + "," + pt.y.ToString() + "\r\n");
Robot.robot.BASE_ROULANTE.direction = forceDir;
// AdjustToAngle == True must be handled here
double angle = 0;
double deltaX, deltaY, deltaTheta, alpha;
deltaX = pt.x - this.GetPosition().x;
deltaY = pt.y - this.GetPosition().y;
deltaTheta = 180 / System.Math.PI * System.Math.Atan2(deltaY, deltaX);
if (deltaY == 0)
{
if (deltaX > 0)
{
deltaTheta = 0;
}
else
{
deltaTheta = 180;
}
}
if (deltaTheta < 0)
{
deltaTheta = 360 + deltaTheta;
}
// We now have the good theta (between 0 and 360)
if (forceDir == OBSTACLE_DIRECTION.AVANT)
{
Informations.printInformations(Priority.HIGH, "Going en avant");
if (this.GetPosition().theta % 360 - deltaTheta % 360 > 180) // That means we have to turn atrigo
{
Rotate(convertTo180(360 - this.GetPosition().theta % 360 - deltaTheta % 360));
// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
else
{
Rotate(convertTo180(-this.GetPosition().theta % 360 + deltaTheta % 360));
// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
deltaX = pt.x - this.GetPosition().x;
deltaY = pt.y - this.GetPosition().y;
Avance((int)System.Math.Sqrt(deltaX * deltaX + deltaY * deltaY));
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
//position = new PointOriente(pt.x, pt.y, deltaTheta);
angle = deltaTheta;
}
if (forceDir == OBSTACLE_DIRECTION.ARRIERE)
{
Informations.printInformations(Priority.HIGH, "Going en arriere");
if (this.GetPosition().theta % 360 - deltaTheta > 180)
{//turn antitrigo
Rotate(convertTo180(-convertTo360(this.GetPosition().theta) + 180 - convertTo360(deltaTheta)));
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
else
{
Rotate(convertTo180(180 - convertTo360(this.GetPosition().theta) + convertTo360(deltaTheta)));
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
deltaX = pt.x - this.GetPosition().x;
deltaY = pt.y - this.GetPosition().y;
Avance(-(int)System.Math.Sqrt(deltaX * deltaX + deltaY * deltaY));
//position = new PointOriente(pt.x, pt.y, 180+deltaTheta%360); // check the angle
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
if (adjustToAngle)
{
Informations.printInformations(Priority.LOW, "Starting to adjust angle");
Rotate((convertTo180(-GetPosition().theta + pt.theta)));
//Robot.robot.BASE_ROULANTE.MoveCompleted.WaitOne();// waiting for the move to be completed
if (this.isPaused)
{
return(false); // MUST BE CHECKED AFTER EACH WaitOne!!!!
}
}
Informations.printInformations(Priority.HIGH, "DOne with gotoForced dir.");
return(true);
}
public ActionBaseRoulante BuildActionBaseRoulante_GOTO_ANGLE(PointOriente pt, OBSTACLE_DIRECTION forcedDirection)
{
return(new ActionBaseRoulante(this.description, new Robot.composants.BaseRoulante.Mouvement(pt, true, forcedDirection)));
}
