Diff: Robots/Strategie_big.cpp

Revision:

15:c2fc239e85df

Parent:

14:c8fc06c4887f

Child:

16:7321fb3bb396

--- a/Robots/Strategie_big.cpp	Fri Mar 31 16:20:26 2017 +0000
+++ b/Robots/Strategie_big.cpp	Thu May 11 12:55:52 2017 +0000
@@ -1,3 +1,4 @@
+
 #include "StrategieManager.h"
 #ifdef ROBOT_BIG
 #include "Config_big.h"
@@ -9,8 +10,8 @@
 /* DESCRIPTION  : Permet de faire la funny action en fin de partie                      */
 /****************************************************************************************/
 void doFunnyAction(void) {
-    AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
-    AX12_processChange();
+   /* AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
+    AX12_processChange();*/
     
 }
 
@@ -22,88 +23,138 @@
     CANMessage msgTx=CANMessage();
     switch(id) {
         
-        case 100://Ouvrir les portes avant
-            AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE);
-            AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE);
-            AX12_processChange();
+        case 100://preparation prise
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=1;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_GAUCHE) speed = BRAS_DROIT;
+                    else speed = BRAS_GAUCHE;
+                }
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
         break;
-        case 101://Fermer les portes avant
-            AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER);
-            AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_FERMER);
-            AX12_processChange();
+        case 101://stockage haut
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=2;
+            msgTx.data[1]=speed;
+        
+            can1.write(msgTx);
         break;
         
-        case 102://Remonter le peigne
-            AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP);
-            SendRawId(0x123);
-            AX12_processChange();
+        case 102://stockage bas
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=3;
+            msgTx.data[1]=speed;
+        
+            can1.write(msgTx);
         break;
-        case 103://Descendre le peigne
-            AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN);
-            AX12_processChange();
+        case 103://deposer
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=4;
+            msgTx.data[1]=speed;
+        
+            can1.write(msgTx);
         break;
         
-        case 104://Monter le support ventouse haut
-            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP);
-            AX12_processChange();
-        break;
-        case 105://Descendre le support ventouse haut
-            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_DOWN);
-            AX12_processChange();
+        case 104://preparation depot bas
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=5;
+            msgTx.data[1]=speed;
+        
+            can1.write(msgTx);
         break;
-        case 106://Remonter le support du cone arriere
-            AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE);
-            AX12_processChange();
+        case 105://preparation depot haut
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=6;
+            msgTx.data[1]=speed;
+        
+            can1.write(msgTx);
         break;
-        case 107://Descendre le support du cone arriere
-            AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE);
-            AX12_processChange();
+        case 106://pousser module
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=7;
+            msgTx.data[1]=speed;
+        
+            can1.write(msgTx);
         break;
         
         case 110://Ouvrir la pince arrière haute
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 205);
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 95);
-            AX12_processChange();
+           msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=10;
+            msgTx.data[1]=speed;
+        
+            can1.write(msgTx);
         break;
         case 111://Fermer la pince arrière haute
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 145);
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 155);
-            AX12_processChange();
-            /*waitingAckID = AX12_ID_PINCE_ARRIERE_HAUTE_DROITE;
-            waitingAckFrom = SERVO_AX12_DONE;*/
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=3;
+        
+            // action et le cote selectionné
+            msgTx.data[0]=11;
+            msgTx.data[1]=speed;
+            msgTx.data[2]=angle;
+        
+            can1.write(msgTx);
         break;
         
         case 112://Ouvrir la pince arrière basse
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 215);
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 85);
-            AX12_processChange();
-        break;
-        case 113://Fermer la pince arrière basse
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 145);
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 155);
-            AX12_processChange();
-           /* waitingAckID = AX12_ID_PINCE_ARRIERE_BASSE_DROITE;
-            waitingAckFrom = SERVO_AX12_DONE;*/
-        break;
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
         
-        case 114://Ouvrir les portes arrières
-            AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 250);
-            AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 50);
-            AX12_processChange();
+            // action et le cote selectionné
+            msgTx.data[0]=12;
+            msgTx.data[1]=speed;
+        
+            can1.write(msgTx);
         break;
-        case 115://Fermer les portes arrière
-            //AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145);
-            //AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155);
-            AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 142);
-            AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 158);
-            AX12_processChange();
-            //waitingAckID = AX12_ID_PORTE_ARRIERE_DROITE;
-            //waitingAckFrom = SERVO_AX12_DONE;
-        break;
-        
+
         case 120://Activer les pompes
-            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
-            AX12_processChange();
+            /*AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
+            AX12_processChange();*/
         
             
             msgTx.id=POMPE_PWM;
@@ -161,14 +212,42 @@
         
         case 40: // demande au telemetre la position d'un objet
             //SendRawId(TELEMETRE_RECHERCHE_OBJET);
-            waitingAckID = ACKNOWLEDGE_TELEMETRE;
-            waitingAckFrom = 0;
-            modeTelemetre =1;
+            
+            modeTelemetre = 1;
+           
+            //angle = angle /10;
+            
+            msgTx.id=TELEMETRE_OBJET;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=1;
+            // indice du module sur le terrain
+            msgTx.data[0] = (unsigned char)speed;
+            
+            /*
+            // x sur 2 octets
+            msgTx.data[0]=(unsigned char)speed;
+            msgTx.data[1]=(unsigned char)(speed>>8);
+            // y sur 2 octets
+            msgTx.data[2]=(unsigned char)angle;
+            msgTx.data[3]=(unsigned char)(angle>>8);
+            // theta signé sur 2 octets
+            //msgTx.data[4]=(unsigned char)theta;
+            //msgTx.data[5]=(unsigned char)(theta>>8);
+            msgTx.data[4]=0;
+            msgTx.data[5]=0;*/
+            
+            can1.write(msgTx);
+            
+        break;
         
         case 130://Lancer mouvement de sortie de la zone de départ
-            SendRawId(ACTION_BIG_DEMARRAGE);
-            waitingAckID = ACTION_BIG_DEMARRAGE;
-            waitingAckFrom = INSTRUCTION_END_MOTEUR;
+            msgTx.id=ACTION_BIG_DEMARRAGE;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=1;
+            msgTx.data[0] = (unsigned char)speed;
+            can1.write(msgTx);
         break;
         
         default:
@@ -186,13 +265,13 @@
 void initRobot(void) 
 {
     //Enregistrement de tous les AX12 présent sur la carte
-    AX12_register(5,  AX12_SERIAL2);
+    /*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_register(7,  AX12_SERIAL2);*/
     
     //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
     //AX12_processChange();
@@ -252,6 +331,7 @@
 {
     switch(id)
     {
+        // strat de match
         case 1:
             strcpy(cheminFileStart,"/local/strat1.txt");
             return FileExists(cheminFileStart);
@@ -283,14 +363,24 @@
             strcpy(cheminFileStart,"/local/strat10.txt");
             return FileExists(cheminFileStart);
         
-        //
+        // strat de demo
         case 0x10:
             strcpy(cheminFileStart,"/local/moteur.txt");
             return FileExists(cheminFileStart);
         case 0x11:
-            strcpy(cheminFileStart,"/local/moteur2.txt");
+#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;