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