示例#1
0
        // Calcul de l'objectif en fonction de l'etat du robot
        public Track CalculerObjectif(ArduinoBotIA robot)
        {
            Logger.GlobalLogger.debug("Creation d'une trajectoire pour le robot " + robot.ID + " etat Saisie : " + robot.Saisie, 2);;
            if (robot.Saisie) // Si le robot a saisi un cube  , retour a la base
            {
                if (robot.Cube != null)
                {
                    Zone o = _ZonesDepose.Find(Zone.ById(robot.Cube.idZone));
                    //Track t = CreerAstarDepose(robot, o).CalculerTrajectoire();

                    Track t = new Track();
                    t.ajouterPoint(robot.Position);
                    t.ajouterPoint(UtilsMath.CentreRectangle(o.position));

                    robot.SetZoneDepose(o);
                    robot.SetTrace(t);
                    return(t);
                }
                else
                {
                    return(null);
                }
            }
            else
            {
                Objectif o;
                if (robot.Cube != null) // On a deja un objectif
                {
                    o = robot.Cube;
                }
                else // Trouver un objectif
                {
                    o = findObjectifNear(robot.Position);
                }

                //Track t = CreerAstarCube(robot, o).CalculerTrajectoire();

                Track t = new Track();
                t.ajouterPoint(robot.Position);
                t.ajouterPoint(o.position);

                robot.SetObjectif(o);
                o.Robot = robot;
                robot.SetTrace(t);
                return(t);
            }
        }
示例#2
0
        private void tickIA()                                                         // Fonction principale
        {
            if (_bInfosPosition && _bInfosCubes && _bInfosZone && _bInfosZoneTravail) // On a recus tout le necessaire depuis l'image
            {
                // foreach (ArduinoBotIA Robot in _ListArduino)
                for (int indexRobot = 0; indexRobot < _ListArduino.Count; indexRobot++)
                {
                    ArduinoBotIA   Robot     = _ListArduino[indexRobot];
                    ArduinoBotComm RobotComm = _ArduinoManager.getArduinoBotById(Robot.ID);

                    if (RobotComm != null && RobotComm.Connected) // Le robot est déja connecté
                    {
                        // Faire des verification et envoi des messages
                        if (Robot.PositionValide) // On a bien une position valide
                        {
                            if (Robot.LastAction == ActionRobot.ROBOT_AUTONOME)
                            {
                                continue;                                                                     // Le robot est en mode autonome (on devrais verifier si il ne séloigne pas de l'objectif )
                            }
                            if (Robot.Trace != null)                                                          // On a un tracé attribué au robot
                            {
                                if (!Robot.Saisie && !_TrackMaker.Cubes.Exists(Objectif.ById(Robot.Cube.id))) // Le cube n'existe plus ( téléportation), et on ne l'as pas saisis
                                {
                                    // Reinit pour recalcul de trajectoire
                                    Robot.SetZoneDepose(null);
                                    Robot.SetObjectif(null);
                                    Robot.SetTrace(null);
                                    Robot.Saisie = false;
                                }

                                if (checkProximiteObjectif(Robot)) // Proximité de l'objectif ?
                                {
                                    Logger.GlobalLogger.debug("Proximité detectée ! ");
                                    if (Robot.Saisie) // On a un cube ? si oui on le dépose
                                    {
                                        MessageProtocol mess = MessageBuilder.createOpenClawMessage();
                                        _AutomateComm.PushSendMessageToArduino(mess, RobotComm);
                                        Robot.LastAction = ActionRobot.ROBOT_PINCE;

                                        Robot.SetZoneDepose(null);
                                        Robot.SetObjectif(null);
                                        Robot.SetTrace(null);
                                        Robot.Saisie = false;
                                    }
                                    else
                                    {
                                        // Passer en Autonome
                                        MessageProtocol mess = MessageBuilder.createModeAutoMessage();
                                        _AutomateComm.PushSendMessageToArduino(mess, RobotComm);
                                        Robot.LastAction = ActionRobot.ROBOT_AUTONOME;
                                    }
                                    Robot.LastActionTime = DateTime.Now;
                                }

                                else
                                {
                                    Logger.GlobalLogger.debug("Proximité non detectée ! ");
                                    PositionElement NearestPositionTroncon;
                                    if (checkSuiteItineraire(Robot.ID, out NearestPositionTroncon)) // On est proche du point de passage ?
                                    {
                                        Logger.GlobalLogger.debug("Point Suivant ");
                                        removeBeforePoint(Robot.Trace, NearestPositionTroncon); // On supprime les anciens points
                                    }



                                    if (!checkProximiteTrace(Robot.ID)) // On est proche du tracé ?
                                    {
                                        // Calcul de la différence d'orientation
                                        if (Math.Abs(diffOrientation(Robot, Robot.Trace)) > _differenceMaxOrientation) // Différence suppérieur de 15 degreé entre le robot et l'angle de la droite
                                        {
                                            Logger.GlobalLogger.debug("Orientation différente ");
                                            if (Robot.LastAction != ActionRobot.ROBOT_TOURNER || (DateTime.Now - Robot.LastActionTime) > TimeSpan.FromSeconds(5)) // On etait pas en train de tourner ou ça fait plus de 5 secondes
                                            {
                                                // Faire trouner le robot
                                                double angle = diffOrientation(Robot, Robot.Trace);

                                                if (angle < 0) // Si inférieur a 0° alors tourner a gauche
                                                {
                                                    MessageProtocol mess = MessageBuilder.createTurnMessage(true, (byte)Math.Abs(angle));
                                                    _AutomateComm.PushSendMessageToArduino(mess, RobotComm);
                                                }
                                                else // sinon touner a droite
                                                {
                                                    MessageProtocol mess = MessageBuilder.createTurnMessage(false, (byte)Math.Abs(angle));
                                                    _AutomateComm.PushSendMessageToArduino(mess, RobotComm);
                                                }
                                                Logger.GlobalLogger.info("Changement d'angle : " + angle);
                                                Robot.LastAction     = ActionRobot.ROBOT_TOURNER;
                                                Robot.LastActionTime = DateTime.Now;
                                            }
                                            // Tourner pour se placer dans le bon sens
                                        }
                                        else
                                        {
                                            // Si oui on continue a se deplacer
                                            foreach (ArduinoBotIA RobotProche in _ListArduino)                                                  // tester la presence d'autre robot a proximité
                                            {
                                                if (RobotProche.ID == Robot.ID)                                                                 // Soi meme
                                                {
                                                    continue;                                                                                   // suivant
                                                }
                                                if (UtilsMath.DistanceEuclidienne(Robot.Position, RobotProche.Position) < _seuilProximiteRobot) // On est trop proche d'un autre robot
                                                {
                                                    if (RobotProche.LastAction != ActionRobot.ROBOT_ARRET)                                      // L'autre robot n'est pas en arret
                                                    {
                                                        // nous arreter
                                                        MessageProtocol mess = MessageBuilder.createMoveMessage(true, (byte)0, (byte)0); // STOP
                                                        _AutomateComm.PushSendMessageToArduino(mess, RobotComm);
                                                        Robot.LastAction     = ActionRobot.ROBOT_ARRET;
                                                        Robot.LastActionTime = DateTime.Now;
                                                        break; // sortie de boucle
                                                    }
                                                }
                                            }
                                            if (Robot.LastAction != ActionRobot.ROBOT_DEPLACER || (DateTime.Now - Robot.LastActionTime) > TimeSpan.FromSeconds(5)) // On etait pas en train de se deplacer ou ça fait plus de 10 secondes
                                            {
                                                double distance = UtilsMath.DistanceEuclidienne(Robot.Position, Robot.Trace.Positions[1]);
                                                //Logger.GlobalLogger.debug("Distance : " + (byte)(distance / _ConversionUnit), 5);
                                                MessageProtocol mess = MessageBuilder.createMoveMessage(true, (byte)100, (byte)(distance / _ConversionUnit)); // Avancer a 50% de vitesse
                                                _AutomateComm.PushSendMessageToArduino(mess, RobotComm);

                                                Robot.LastAction     = ActionRobot.ROBOT_DEPLACER;
                                                Robot.LastActionTime = DateTime.Now;
                                            }
                                            else
                                            {
                                                // ne pas renvoyer d'ordre, le robot est en train de se déplacer
                                            }
                                        }
                                    }
                                    else
                                    {
                                        // Si non on recalcule
                                        _TrackMaker.CalculerObjectif(Robot);
                                    }
                                }
                            }
                            else
                            {
                                // Calcul d'un itineraire
                                _TrackMaker.CalculerObjectif(Robot);
                            }
                        }
                        else
                        {// Pas de position valide
                            // Envoyer un stop pour ne pas bouger
                            MessageProtocol mess = MessageBuilder.createMoveMessage(true, 0x00, 0x00);
                            _AutomateComm.PushSendMessageToArduino(mess, RobotComm);
                            Robot.LastAction     = ActionRobot.ROBOT_ARRET;
                            Robot.LastActionTime = DateTime.Now;
                        }
                    }
                }
            }
        }