Exemplo n.º 1
0
        // Trouve l'objectif le plus près de la position (objectif non effectué )
        private Objectif findObjectifNear(PositionElement p)
        {
            Objectif sortie = null;

            double MinDist = -1;

            foreach (Objectif ob in _Cubes)
            {
                // Objectif non fait et non en cours de traitement
                if (ob.Done == false && ob.Robot == null)
                {
                    double dist = UtilsMath.DistanceManhattan(p, ob.position);
                    if (sortie == null) // On a pas encore d'objectif ?
                    {
                        sortie  = ob;
                        MinDist = dist;
                    }
                    else
                    {
                        if (MinDist > dist)
                        {
                            sortie  = ob;
                            MinDist = dist;
                        }
                    }
                }
            }
            return(sortie);
        }
Exemplo n.º 2
0
 public void SetZoneDepose(Zone Depose)
 {
     _Saisie     = true;
     _Objectif   = null;
     _DeposeZone = Depose;
     _Trace      = null;
 }
Exemplo n.º 3
0
 // Choisi si le deplacement en direction d'une zone ou d'un cube
 public void SetObjectif(Objectif ob)
 {
     _Saisie     = false;
     _Objectif   = ob;
     _DeposeZone = null;
     _Trace      = null;
 }
Exemplo n.º 4
0
 // Met a jour ou ajoute un cube
 public void updateCube(Objectif cube)
 {
     if (_Cubes.Exists(Objectif.ById(cube.id)))
     {
         int index = _Cubes.FindIndex(Objectif.ById(cube.id));
         if (cube.position.X == -1 || cube.position.Y == -1) // position invalide , suppression
         {
             _Cubes.RemoveAt(index);
         }
         else
         {
             _Cubes[index] = cube;
         }
     }
     else
     {
         _Cubes.Add(cube);
     }
 }
Exemplo n.º 5
0
 // Proximité de l'objectif ou de la zone
 private bool checkProximiteObjectif(ArduinoBotIA Robot)
 {
     if (Robot.Saisie) // le robot à un cube de saisie
     {
         Zone Depose = Robot.DeposeZone;
         if (UtilsMath.DistanceEuclidienne(Robot.Position, UtilsMath.CentreRectangle(Depose.position)) < _seuilProximiteObjectif)
         {
             Logger.GlobalLogger.info("Robot proche de la zone de dépose ! id : " + Robot.ID);
             return(true);
         }
     }
     else
     {
         Objectif Cube = Robot.Cube;
         if (UtilsMath.DistanceEuclidienne(Robot.Position, Cube.position) < _seuilProximiteObjectif)
         {
             Logger.GlobalLogger.info("Robot proche du cube ! id : " + Robot.ID);
             return(true);
         }
     }
     return(false);
 }
Exemplo n.º 6
0
        // Creer un astar avec les zones et les positions correctes
        private AStar CreerAstarCube(ArduinoBotIA robot, Objectif Cube)
        {
            AStar AS = new AStar(Cube.position, robot.Position, _ZoneTravail);

            // Ajout des autres cubes en obstacles
            foreach (Objectif o in _Cubes)
            {
                if (o.id != Cube.id)
                {
                    if (o.Robot != null)
                    {
                        AS.AddObstacle(o.position);
                    }
                }
            }

            // Ajout des Zones en Obstacles
            foreach (Zone o in _ZonesDepose)
            {
                AS.AddObstacle(UtilsMath.CentreRectangle(o.position));
            }

            return(AS);
        }
Exemplo n.º 7
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;
                        }
                    }
                }
            }
        }