// Verifie si un robot est bien proche du tracé qu'il doit effectuer
private bool checkProximiteTrace(byte idRobot)
{
// On a bien un robot avec ce numéro
if (_ListArduino.Exists(ArduinoBotIA.ById(idRobot)))
{
ArduinoBotIA Robot = _ListArduino.Find(ArduinoBotIA.ById(idRobot));
// Le robot a bien un tracé définit
if (Robot.Trace != null)
{
Track tr = Robot.Trace;
double minDistance = -1;
for (int i = 0; i < tr.Positions.Count - 1; i++)
{
// Trouve la distance la plus petite entre la position actuelle et les traits du tracé
double tmp = UtilsMath.FindDistanceToSegment(Robot.Position, tr.Positions[i], tr.Positions[i + 1]);
if (minDistance == -1 || minDistance > tmp)
{
minDistance = tmp;
}
}
if (minDistance >= _DistanceMaximumRecal)
{
Logger.GlobalLogger.info("Robot trop éloigné de son tracé, Recalcul de l'itinéraire ");
return(true);
}
}
}
return(false);
}
// Verifie si le robot est proche d'un point de l'itinéraire pour changer passer a la suite
private bool checkSuiteItineraire(byte idRobot, out PositionElement NearestPositionTroncon)
{
NearestPositionTroncon = new PositionElement();
// On a bien un robot avec ce numéro
if (_ListArduino.Exists(ArduinoBotIA.ById(idRobot)))
{
ArduinoBotIA Robot = _ListArduino.Find(ArduinoBotIA.ById(idRobot));
// Le robot a bien un tracé définit
if (Robot.Trace != null)
{
Track tr = Robot.Trace;
for (int i = tr.Positions.Count - 1; i >= 0; i--) // On parcours le tracé depuis la fin pour trouver le plus proche de lobjectif
{
// On est assez proche du point, on passe à la suite
if (UtilsMath.DistanceEuclidienne(tr.Positions[i], Robot.Position) < _RadiusNextItineraire)
{
NearestPositionTroncon = tr.Positions[i];
return(true);
}
}
}
}
return(false);
}
void _AutomateComm_OnNewTrameArduinoReceived(object sender, NewTrameArduinoReceveidEventArgs e)
{
byte IDRobot = e.Source.id;
ArduinoBotIA Rob;
if (_ListArduino.Exists(ArduinoBotIA.ById(IDRobot)))
{
Rob = _ListArduino.Find(ArduinoBotIA.ById(IDRobot));
}
else
{
Rob = new ArduinoBotIA(IDRobot);
_ListArduino.Add(Rob);
}
switch (e.Message.headerMess)
{
case (byte)EMBtoPCmessHeads.ASK_CONN:
// Rien de spécial sur la demande de connexion
break;
case (byte)EMBtoPCmessHeads.AUTO_MODE_OFF:
// Fin de mode auto / le robot doit avoir pris le cube
if (Rob.LastAction == ActionRobot.ROBOT_AUTONOME)
{
Rob.LastAction = ActionRobot.ROBOT_ARRET;
Rob.Saisie = true;
Rob.SetTrace(null);
// Une fois le cube saisie
}
else
{
Logger.GlobalLogger.error("Fin de mode autnome qui n'as pas été lancé par le serveur !" + Rob.ID);
}
break;
case (byte)EMBtoPCmessHeads.RESP_SENSOR:
Logger.GlobalLogger.info("Réponse de capteur ? : : " + e.Message.ToString());
break;
default:
Logger.GlobalLogger.error("Message reçu qui ne devrais pas être remonter dans L'IA : " + e.Message.ToString());
break;
}
tickIA();
}
public void UpdatePositionRobots(List <PositionRobot> Positions)
{
_bInfosPosition = true;
// Mettre a jour la liste des robots avec les infos en provenance de l'image
foreach (PositionRobot p in Positions)
{
ArduinoBotIA Robot = _ListArduino.Find(ArduinoBotIA.ById((byte)p.Identifiant));
if (Robot != null)
{
int index = _ListArduino.IndexOf(Robot);
if (p.Position.X == -1 || p.Position.Y == -1)
{
_ListArduino.RemoveAt(index);
}
else
{
ArduinoBotIA Robot2 = _ListArduino[index];
Robot2.Position = p.Position;
Robot2.Angle = p.Angle;
_ListArduino[index] = Robot2;
}
}
else
{
if (p.Position.X != -1 && p.Position.Y != -1)
{
ArduinoBotIA Robot2 = new ArduinoBotIA((byte)p.Identifiant);
Robot2.Position = p.Position;
Robot2.Angle = p.Angle;
_ListArduino.Add(Robot2);
}
}
}
// Declencher un tick de l'IA
tickIA();
}