Diff: Robots/Strategie_big.cpp

Revision:

10:1964bb91b925

Parent:

1:7e925468f9d9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robots/Strategie_big.cpp	Tue Mar 03 16:41:33 2020 +0000
@@ -0,0 +1,305 @@
+#include "global.h"
+#ifdef ROBOT_BIG
+
+unsigned short x;
+unsigned short y;
+unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
+
+/****************************************************************************************/
+/* FUNCTION NAME: doFunnyAction                                                         */
+/* DESCRIPTION  : Permet de faire la funny action en fin de partie                      */
+/****************************************************************************************/
+void doFunnyAction(void)
+{
+    //envoie de la funny action
+    // 0x007, 01, 01
+    CANMessage msgTx=CANMessage();
+    msgTx.id=GLOBAL_FUNNY_ACTION;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.len=2;
+    msgTx.data[0]=0x01;
+    msgTx.data[1]=0x01;
+    can1.write(msgTx);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: doAction                                                              */
+/* DESCRIPTION  : Effectuer une action specifique                                       */
+/****************************************************************************************/
+unsigned char doAction(unsigned char id, unsigned short var1, short var2)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    //affichage_debug(id);
+
+    switch(id) {
+        /////////////////////////////////////////////////////////100 à 108 : ACTIONS HERKULEX/////////////////////////////////////////////
+        case 118:
+            SendRawId(VENTOUSE_AV_CENTRE_BALANCE);
+            break;
+
+        case 201:
+            unsigned char var_tempo;
+            var_tempo = (unsigned char)var1;//0auto 1forceon 2 forceoff
+            SendMsgCan(ASCENSEUR, &var_tempo,1);
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break;
+
+        case 202:
+            msgTx.id=VIDER_CONVOYEUR;
+            msgTx.len=1;
+            msgTx.data[0]=(unsigned char)var1; //
+            can2.write(msgTx);
+            break;
+
+        case 203:
+            x = var1;
+            if(InversStrat == 1) {
+                y = 3000 - var2;
+            } else {
+                y = var2;
+            }
+            Send2Short(GOLDENIUM_AVANT, x, y);
+            break;
+
+        case 204:
+            unsigned char arg_tempo;
+            if(InversStrat == 1) {
+                switch(var1) {
+                    case AV_DROIT:
+                        arg_tempo = AV_GAUCHE;
+                        break;
+                    case AV_GAUCHE:
+                        arg_tempo = AV_DROIT;
+                        break;
+                    default :
+                        arg_tempo =(unsigned char)var1;
+                        break;
+                }
+
+            } else arg_tempo =(unsigned char)var1;
+            SendMsgCan(HACHEUR_RELEASE_ATOM, &arg_tempo,1);
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break;
+
+        case 205:
+            SendRawId(PRESENTOIR_AVANT);
+            break;
+            
+        case 206:
+            SendMsgCan(RATEAU, (unsigned char*)&var1,1);
+            break;
+
+
+        case 150:
+            SCORE_GR+=var1;
+            SCORE_GLOBAL=SCORE_GR+SCORE_PR;
+            //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL);
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            break;
+
+
+        case 11://0 Désactiver le stop,1 Activer le stop saut de strat,2 Activer le stop avec evitement
+            isStopEnable =(unsigned char) var1;
+            // SendMsgCan(0x5BC, &isStopEnable,1);
+            waitingAckFrom = 0;
+            waitingAckID =0;
+            break;
+
+        case 20://Désactiver l'asservissement
+            setAsservissementEtat(0);
+            break;
+
+        case 21://Activer l'asservissement
+            setAsservissementEtat(1);
+            break;
+
+        case 22://Changer la vitesse du robot
+            SendSpeed(var1);
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            wait(0.2);
+            break;
+
+        case 23:
+            SendAccel((unsigned short)var1,(unsigned short)var2);//,(unsigned short)arg2, (unsigned short)arg2);
+            wait_us(200);
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            break;
+
+        case 30://Action tempo
+            wait_ms(var1);
+            waitingAckFrom = 0;
+            waitingAckID = 0;
+            break;
+
+
+
+        default:
+            return 0;//L'action n'existe pas, il faut utiliser le CAN
+
+    }
+    return 1;//L'action est spécifique.
+
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: initRobot                                                             */
+/* DESCRIPTION  : initialiser le robot                                                  */
+/****************************************************************************************/
+void initRobot(void)
+{
+    //Enregistrement de tous les AX12 présent sur la carte
+    /*AX12_register(5,  AX12_SERIAL2);
+    AX12_register(18, AX12_SERIAL2);
+    AX12_register(13, AX12_SERIAL2);
+    AX12_register(1,  AX12_SERIAL1);
+    AX12_register(11,  AX12_SERIAL1);
+    AX12_register(8,  AX12_SERIAL1);
+    AX12_register(7,  AX12_SERIAL2);*/
+
+    //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
+    //AX12_processChange();
+    //runRobotTest();
+
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: initRobotActionneur                                                   */
+/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
+/****************************************************************************************/
+void initRobotActionneur(void)
+{
+    /*doAction(100,1,0);
+    doAction(100,2,0);
+    doAction(110,0,0);
+    doAction(120,0,0);
+    doAction(131,0,0);*/
+
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: runTest                                                               */
+/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
+/****************************************************************************************/
+void runRobotTest(void)
+{
+    /*
+    int waitTime = 500;
+
+    //Test des AX12 dans l'ordre
+    doAction(111,0,0);//Fermeture pince arrière haute
+    wait_ms(waitTime);
+    doAction(110,0,0);//Ouverture pince arrière haute
+    wait_ms(waitTime);
+    doAction(113,0,0);//Fermeture pince arrière basse
+    wait_ms(waitTime);
+    doAction(112,0,0);//Ouverture pince arrière basse
+    wait_ms(waitTime);
+    doAction(115,0,0);//Fermeture porte arrière
+    wait_ms(waitTime);
+    doAction(114,0,0);//Ouverture porte arrière
+    wait_ms(waitTime);
+    doAction(101,0,0);//Fermer les portes avant
+    wait_ms(waitTime);
+    doAction(100,0,0);//Ouvrir les portes avant
+    wait_ms(waitTime);
+    doAction(103,0,0);//Descendre le peigne
+    wait_ms(waitTime);
+    doAction(102,0,0);//Remonter le peigne*/
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: SelectStrategy                                                        */
+/* DESCRIPTION  : Charger le fichier de stratégie correspondante à un id                */
+/* RETURN       : 0=> Erreur, 1=> OK si le fichier existe                               */
+/****************************************************************************************/
+/*int SelectStrategy(unsigned char id)
+{
+    switch(id)
+    {
+        // strat de match
+        case 1:
+            strcpy(cheminFileStart,"/local/strat1.txt");
+            return FileExists(cheminFileStart);
+        case 2:
+            strcpy(cheminFileStart,"/local/strat2.txt");
+            return FileExists(cheminFileStart);
+        case 3:
+            strcpy(cheminFileStart,"/local/strat3.txt");
+            return FileExists(cheminFileStart);
+        case 4:
+            strcpy(cheminFileStart,"/local/strat4.txt");
+            return FileExists(cheminFileStart);
+        case 5:
+            strcpy(cheminFileStart,"/local/strat5.txt");
+            return FileExists(cheminFileStart);
+        case 6:
+            strcpy(cheminFileStart,"/local/strat6.txt");
+            return FileExists(cheminFileStart);
+        case 7:
+            strcpy(cheminFileStart,"/local/strat7.txt");
+            return FileExists(cheminFileStart);
+        case 8:
+            strcpy(cheminFileStart,"/local/strat8.txt");
+            return FileExists(cheminFileStart);
+        case 9:
+            strcpy(cheminFileStart,"/local/strat9.txt");
+            return FileExists(cheminFileStart);
+        case 10:
+            strcpy(cheminFileStart,"/local/strat10.txt");
+            return FileExists(cheminFileStart);
+
+        // strat de demo
+        case 0x10:
+            strcpy(cheminFileStart,"/local/moteur.txt");
+            return FileExists(cheminFileStart);
+        case 0x11:
+#ifdef ROBOT_BIG
+            strcpy(cheminFileStart,"/local/bras.txt");
+#else
+            strcpy(cheminFileStart,"/local/porteAvant.txt");
+#endif
+            return FileExists(cheminFileStart);
+        case 0x12:
+#ifdef ROBOT_BIG
+            strcpy(cheminFileStart,"/local/balancier.txt");
+#else
+            strcpy(cheminFileStart,"/local/mainTourneuse.txt");
+#endif
+            return FileExists(cheminFileStart);
+        default:
+            strcpy(cheminFileStart,"/local/strat1.txt");
+            return 0;
+    }
+}*/
+
+/****************************************************************************************/
+/* FUNCTION NAME: needToStop                                                            */
+/* DESCRIPTION  : Savoir si il faut autoriser le stop du robot via balise               */
+/****************************************************************************************/
+unsigned char needToStop(void)
+{
+    return isStopEnable;
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: doBeforeEndAction                                                     */
+/* DESCRIPTION  : Terminer les actions du robot 1s avant la fin du match                */
+/****************************************************************************************/
+void doBeforeEndAction(void)
+{
+    doAction(110,0,0);//Ouverture pince arrière haute
+    doAction(112,0,0);//Ouverture pince arrière basse
+    doAction(114,0,0);//Ouverture porte arrière
+    doAction(100,0,0);//Ouvrir les portes avant
+    doAction(102,0,0);//Remonter le peigne
+}
+
+#endif