Beispiel #1
0
        etatBR robotGoToXY(ushort x, ushort y, sens s, bool boolDetection = false, int speed = 2)
        {
            etatBR retour;

            if (boolDetection)
            {
                // on passe le sens "dir" au timer via la variable "state"
                // analogue au timeout-callback pour les amoureux du js
                //Timer t = new Timer(new TimerCallback(Detecter), s, 0, 1000);
                obstacle = false; // paramètre pour savoir si il y'a bien un obstacle
                var thDetection = new Thread(() =>
                {
                    while (true)
                    {
                        Detecter(s);
                        //Thread.Sleep(20);
                    }
                });
                thDetection.Start();

                retour = BaseRoulante.allerDect(x, y, s, speed);// x,y,s
                thDetection.Suspend();
                obstacle = false;
                //t.Dispose();
            }
            else
            {
                retour = BaseRoulante.allerEn(x, y, s, speed);
            }
            Debug.Print("pos_visee " + x + " " + y);

            return(retour);
        }
Beispiel #2
0
        public void Detecter(object o)
        {
            sens dir = (sens)o;

            // si on avance, les ultrasons sont utiles

            if (dir == sens.avancer)
            {
                if ((!IR.AVG.Read() || !IR.AVD.Read()))// infrarouge OK.. et c'est une condition et && obstacleUS
                {
                    obstacle = true;
                }
                else
                {
                    obstacle = false;
                }
            }
            // si on recule, les ultrasons ne sont plus utiles
            else
            {
                // on teste les capteurs IR arrières
                if (!IR.ARG.Read() || !IR.ARD.Read())
                {
                    obstacle = true;
                }
                else
                {
                    obstacle = false;
                }
            }
        }
Beispiel #3
0
        //public void recalagePosX(int angle, int x,int speed,sens s)
        //{
        //    bool reponse = true;
        //    m_posBR.alpha = angle;
        //    m_posBR.y = x;
        //    int r = 0;
        //    int distanceReelle=0;
        //    int AdistanceReelle=1;
        //    m_kangaroo.allerEn((int)(s)*100, speed, unite.mm);
        //    Thread.Sleep(150);
        //    while (reponse)
        //    {
        //        getDistanceParcourue(ref distanceReelle);
        //        if (distanceReelle!=AdistanceReelle) r=0;
        //        if (distanceReelle==AdistanceReelle) r++;
        //        if (r == 3)
        //        {
        //            m_kangaroo.start(mode.drive);
        //            //m_kangaroo.allerEn(0, speed, unite.mm);
        //            reponse = false;

        //        }
        //        Thread.Sleep(30);
        //    }
        //    Thread.Sleep(1000);
        //}

        public void recalagePosX(int angle, int x, int speed, sens s, int temps)
        {
            m_posBR.alpha = angle;
            m_posBR.y     = x;

            m_kangaroo.allerEn((int)(s) * 150, speed, unite.mm);
            Thread.Sleep(temps);
            m_kangaroo.start(mode.drive);
        }
Beispiel #4
0
 public void recalagePosY(int angle, int y, int speed, sens s, int temps)
 {
     m_posBR.alpha = angle;
     m_posBR.y     = y;
     // essaye de s'avancer 10 cm
     m_kangaroo.allerEn((int)(s) * 150, speed, unite.mm);
     Thread.Sleep(temps);
     m_kangaroo.start(mode.drive);
 }
Beispiel #5
0
        public int setMovingSpeed(sens dir, int vitesse)
        {
            m_sens = dir;
            byte len, error = 1;
            int  value = vitesse + (int)dir;

            byte[] buf = { 0x20, (byte)(value), (byte)(value >> 8) };
            sendCommand(Instruction.AX_WRITE_DATA, buf);
            getReponse(out len, out error, null);
            return((int)error);
        }
Beispiel #6
0
        public int stop()
        {
            m_sens = 0;
            byte len, error = 1;
            int  value = 0;

            byte[] buf = { 0x20, (byte)(value), (byte)(value >> 8) };
            sendCommand(Instruction.AX_WRITE_DATA, buf);
            getReponse(out len, out error, null);
            return((int)error);
        }
Beispiel #7
0
        public void Detecter(object o)
        {
            sens dir = (sens)o;

            // si on avance, les ultrasons sont utiles

            if (dir == sens.avancer)
            {
                // on teste les capteurs IR avants puis le capteur laser en appui

                /*
                 * double distance = 0d;
                 * bool obstacleUS = false;*/
                // mesure une distance moyenne avec 5 mesures rapides
                //Ultrason désactivé pour l'instant, ils prennent beaucoup trop de temps pour acquérir l'information.

                /*
                 * m_ultrason.getDistance(1, ref distance);
                 * if (distance < 30 && distance != -1)
                 *  obstacleUS = true;
                 */

                if ((!m_IR.AVG.Read() || !m_IR.AVD.Read()))// infrarouge OK.. et c'est une condition et && obstacleUS
                {
                    //m_baseRoulante.stop();
                    obstacle = true;
                }
                else
                {
                    obstacle = false;
                }
            }
            // si on recule, les ultrasons ne sont plus utiles
            else
            {
                // on teste les capteurs IR arrières
                if (!m_IR.ARG.Read() || !m_IR.ARD.Read())
                {
                    //Debug.Print("Détection obstacle après");
                    //m_baseRoulante.stop();
                    obstacle = true;
                }
                else
                {
                    obstacle = false;
                }
            }
        }
Beispiel #8
0
 void recalageY(int angle, int y, sens s, int speed, int temps)
 {
     BaseRoulante.recalagePosY(angle, y, speed, s, temps);
 }
Beispiel #9
0
 void recalageX(int angle, int x, sens s, int speed, int temps)
 {
     BaseRoulante.recalagePosX(angle, x, speed, s, temps);
 }
Beispiel #10
0
        public etatBR allerDect(double x, double y, sens s, int speed = 1)
        {
            int posCodeur = 0;
            int erreur = 0;
            int distanceConsigne = 0, distanceReelle = 0, distanceReelle_tm1 = 0, distanceStop = 0, distanceSF = 0;
            int alphaConsigne = 0, alphaReel = 0, alphaReel_tm1 = 0;
            int delta = 0;
            int ecart_t = 0, ecart_tm1 = 0;
            int dureeBlocage = 0;



            alphaConsigne = (int)(System.Math.Atan2((y - m_posBR.y), (x - m_posBR.x)) * 180 / System.Math.PI) - m_posBR.alpha;  //angle en degre
            if (s == sens.reculer)
            {
                alphaConsigne = (int)(alphaConsigne + 180);
            }
            alphaConsigne = recallerAngle(alphaConsigne);
            //   m_status= Tourner(alphaConsigne);
            m_status = 0;
            int a = 1;

            m_kangaroo.Tourner(alphaConsigne);
            //attente d'être arrive ou bloque ou stoppe
            do
            {
                alphaReel_tm1 = alphaReel;
                //  Thread.Sleep(1000);
                erreur = getAngleTourne(ref alphaReel);
                if (erreur == 0xE3)
                {
                    alphaReel = alphaReel_tm1;
                    m_status  = etatBR.bloque;
                    //relai.TurnOn();
                    Thread.Sleep(500);
                    //relai.TurnOff();
                    Thread.Sleep(500);
                    m_kangaroo.init();
                }
                else
                {
                    delta = System.Math.Abs(alphaConsigne - alphaReel);
                    if (delta < 3)
                    {
                        m_status = etatBR.arrive;
                        Thread.Sleep(200);
                        getAngleTourne(ref alphaReel);
                    }
                }
            } while (m_status != etatBR.arrive && m_status != etatBR.bloque && m_status != etatBR.stope);
            delta            = 0;
            m_status         = 0;
            distanceConsigne = (int)s * (int)System.Math.Sqrt(System.Math.Pow((x - m_posBR.x), 2) + System.Math.Pow((y - m_posBR.y), 2));
            m_kangaroo.allerEn(distanceConsigne, speed, unite.mm);//unite.mm

            do
            {
                if (GrandRobot.obstacle)
                {
                    Debug.Print("entrée de la fonction check");
                    getDistanceParcourue(ref distanceStop);

                    //m_kangaroo.allerEn(0, 1, unite.mm);
                    m_kangaroo.start(mode.drive);
                    distanceSF += distanceStop;
                    while (GrandRobot.obstacle)
                    {
                        Thread.Sleep(100);
                    }
                    Debug.Print(distanceSF + "");
                    // m_kangaroo.allerEn(distanceConsigne + distanceSF, speed, unite.mm);//distanceConsigne - distanceSF
                    allerDect(x, y, s);
                    Debug.Print("sortie de la fonction check");
                }
                distanceReelle_tm1 = distanceReelle;
                erreur             = getDistanceParcourue(ref distanceReelle);
                if (distanceReelle == 0)
                {
                    dureeBlocage++;

                    if (dureeBlocage >= 10)
                    {
                        distanceReelle = distanceReelle_tm1;
                        m_status       = etatBR.bloque;
                        m_kangaroo.start(mode.drive);
                    }
                }
                else
                {
                    dureeBlocage = 0;
                }

                delta = System.Math.Abs(distanceConsigne - distanceReelle - (int)distanceSF);//(int) s*distanceSF
                if (delta < 3)
                {
                    m_status = etatBR.arrive;
                    Thread.Sleep(200);
                    erreur = getDistanceParcourue(ref distanceReelle);
                }

                if (erreur == 0xE3)
                {
                    distanceReelle = distanceReelle_tm1;
                    m_status       = etatBR.bloque;
                    //relai.TurnOn();
                    Thread.Sleep(500);
                    //relai.TurnOff();
                    Thread.Sleep(500);
                    m_kangaroo.init();
                }
            } while (m_status != etatBR.arrive && m_status != etatBR.bloque && m_status != etatBR.stope);
            m_posBR.alpha = m_posBR.alpha + alphaReel;
            m_posBR.x     = m_posBR.x + (int)((-distanceReelle - (int)distanceSF) * System.Math.Cos(m_posBR.alpha * System.Math.PI / 180)); //distanceReelle + (int)distanceSF
            m_posBR.y     = m_posBR.y + (int)((-distanceReelle - (int)distanceSF) * System.Math.Sin(m_posBR.alpha * System.Math.PI / 180)); //distanceReelle + (int)distanceSF
            return(m_status);
        }
Beispiel #11
0
        public etatBR allerEn(double x, double y, sens s, int speed = 6)
        {
            int erreur = 0;
            int distanceConsigne = 0, distanceReelle = 0, distanceReelle_tm1 = 0;
            int alphaConsigne = 0, alphaReel = 0, alphaReel_tm1 = 0;
            int delta        = 0;
            int dureeBlocage = 0;



            alphaConsigne = (int)(System.Math.Atan2((y - m_posBR.y), (x - m_posBR.x)) * 180 / System.Math.PI) - m_posBR.alpha;  //angle en degre
            if (s == sens.reculer)
            {
                alphaConsigne = (int)(alphaConsigne + 180);
            }
            alphaConsigne = recallerAngle(alphaConsigne);
            //   m_status= Tourner(alphaConsigne);
            m_status = 0;
            m_kangaroo.Tourner(alphaConsigne);
            //attente d'être arrive ou bloque ou stoppe
            do
            {
                alphaReel_tm1 = alphaReel;
                //  Thread.Sleep(1000);
                erreur = getAngleTourne(ref alphaReel);
                Debug.Print("" + erreur);
                if (erreur == 0xE3)
                {
                    alphaReel = alphaReel_tm1;
                    m_status  = etatBR.bloque;
                    //relai.TurnOn();
                    Thread.Sleep(500);
                    //relai.TurnOff();
                    Thread.Sleep(500);
                    m_kangaroo.init();
                }
                else
                {
                    delta = System.Math.Abs(alphaConsigne - alphaReel);
                    Debug.Print("delta " + delta);
                    if (delta < 3)
                    {
                        m_status = etatBR.arrive;
                        // pour l'inertie
                        Thread.Sleep(100);
                        getAngleTourne(ref alphaReel);
                    }
                }
            } while (m_status != etatBR.arrive && m_status != etatBR.bloque && m_status != etatBR.stope);
            delta            = 0;
            m_status         = 0;
            distanceConsigne = (int)s * (int)System.Math.Sqrt(System.Math.Pow((x - m_posBR.x), 2) + System.Math.Pow((y - m_posBR.y), 2));
            Debug.Print("distance à parcourir : " + distanceConsigne);
            m_kangaroo.allerEn(distanceConsigne, speed, unite.mm);
            //attente d'être arrive ou bloque ou stoppe
            do
            {
                distanceReelle_tm1 = distanceReelle;
                erreur             = getDistanceParcourue(ref distanceReelle);
                if (distanceReelle == 0)
                {
                    dureeBlocage++;

                    if (dureeBlocage >= 10)
                    {
                        distanceReelle = distanceReelle_tm1;
                        m_status       = etatBR.bloque;
                        m_kangaroo.start(mode.drive);
                    }
                }
                else
                {
                    dureeBlocage = 0;
                }

                delta = System.Math.Abs(distanceConsigne - distanceReelle);
                if (delta < 3)//5
                {
                    m_status = etatBR.arrive;
                    Thread.Sleep(200);
                    erreur = getDistanceParcourue(ref distanceReelle);
                }

                if (erreur == 0xE3)
                {
                    distanceReelle = distanceReelle_tm1;
                    m_status       = etatBR.bloque;
                    //relai.TurnOn();
                    Thread.Sleep(500);
                    //relai.TurnOff();
                    Thread.Sleep(500);
                    m_kangaroo.init();
                }
            } while (m_status != etatBR.arrive && m_status != etatBR.bloque && m_status != etatBR.stope);

            m_posBR.alpha = m_posBR.alpha + alphaReel;
            m_posBR.x     = m_posBR.x + (int)(distanceReelle * System.Math.Cos(m_posBR.alpha * System.Math.PI / 180));
            m_posBR.y     = m_posBR.y + (int)(distanceReelle * System.Math.Sin(m_posBR.alpha * System.Math.PI / 180));
            Debug.Print("position_x " + m_posBR.x + " position_y" + m_posBR.y);

            /*  m_kangaroo.powerdown(mode.drive);
             * m_kangaroo.powerdown(mode.turn);
             * m_kangaroo.init();*/

            return(m_status);
        }
Beispiel #12
0
        //CHANGE LA VITESSE DU MOTEUR
        public bool setSpeedMotor(int numMoteur, sens direction)
        {
            bool retour = false;
            int i = 0;

            char[] cVitesse = new char[10];
            byte[] commande = new byte[20];
            byte[] reponse = new byte[20];

            for (i = 0; i < 20; i++)
            {
                reponse[i] = 0;
            }

            if (m_serial.IsOpen)
            {

                commande[0] = (byte)'p';
                commande[1] = (byte)'w';
                commande[2] = (byte)'m';
                commande[3] = (byte)' ';
                commande[4] = (byte)numMoteur.ToString().ToCharArray()[0];
                commande[5] = (byte)':';

                // ON MET LA VITESSE(parametre direction: enum sens) DANS UN TABLEAU DE CHAR QUI VA ETRE MIT DANS LA COMMANDE ET VA ETRE ENVOYER
                cVitesse = direction.ToString().ToCharArray();
                for (i = 0; i < cVitesse.Length; i++)
                    commande[6 + i] = (byte)cVitesse[i];
                commande[6 + i] = 0x0d;
                m_serial.Write(commande, 0, 7 + cVitesse.Length);

                if (getReponse(ref reponse) != 0)
                    retour = true;
            }
            return retour;
        }
Beispiel #13
0
 public bool tourner(sens direction)
 {
     return m_serializer.setSpeedMotor(m_numMoteur, direction);
 }