code petit robot pour homologation

Fork of CRAC-Strat_2017_V2 by CRAC Team

Revision:
11:ed13a480ddca
Parent:
10:a788d9cf60f2
Child:
12:14729d584500
--- a/Robots/Strategie_small.cpp	Fri Apr 29 09:14:27 2016 +0000
+++ b/Robots/Strategie_small.cpp	Mon May 02 19:40:59 2016 +0000
@@ -35,77 +35,234 @@
 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
     switch(id) {
         case 101://Descendre le bras pour pecher les poissons
-            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche
-                AX12_setGoal(AX12_ID_BRAS_BASE_INV,200,0x0FF);
+            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+                AX12_setGoal(
+                    AX12_ID_BRAS_BASE_DROIT,
+                    AX12_ANGLE_BRAS_BASE_DROIT_OUVERT,
+                    AX12_SPEED_BRAS_BASE
+                );
                 AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_BASE_DROIT;
+                waitingAckFrom = SERVO_AX12_DONE;
             } else {
-                AX12_setGoal(AX12_ID_BRAS_BASE,140,0x0FF);
+                AX12_setGoal(
+                    AX12_ID_BRAS_BASE_GAUCHE,
+                    AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT,
+                    AX12_SPEED_BRAS_BASE
+                );
                 AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
+                waitingAckFrom = SERVO_AX12_DONE;
             }
         break;
         case 102://Remonter le bras avec les poissons dessus
-            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche
-                AX12_setGoal(AX12_ID_BRAS_BASE_INV,220,0x0FF);
+            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+                AX12_setGoal(
+                    AX12_ID_BRAS_BASE_DROIT,
+                    AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
+                    AX12_SPEED_BRAS_BASE
+                );
                 AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_BASE_DROIT;
+                waitingAckFrom = SERVO_AX12_DONE;
             } else {
-                AX12_setGoal(AX12_ID_BRAS_BASE,100,0x0FF);
+                AX12_setGoal(
+                    AX12_ID_BRAS_BASE_GAUCHE,
+                    AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
+                    AX12_SPEED_BRAS_BASE
+                );
                 AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
+                waitingAckFrom = SERVO_AX12_DONE;
             }
         break;
         case 103://Decendre à 30° 
-            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche
-                AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,90);//Ouverture du bras
+            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+                AX12_setGoal(
+                    AX12_ID_BRAS_BASE_DROIT,
+                    AX12_ANGLE_BRAS_BASE_DROIT_RELACHER,
+                    AX12_SPEED_BRAS_BASE
+                );
                 AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_BASE_DROIT;
+                waitingAckFrom = SERVO_AX12_DONE;
             } else {
-                AX12_setGoal(AX12_ID_BRAS_RELACHEUR,250);//Ouverture du bras
+                AX12_setGoal(
+                    AX12_ID_BRAS_BASE_GAUCHE,
+                    AX12_ANGLE_BRAS_BASE_GAUCHE_RELACHER,
+                    AX12_SPEED_BRAS_BASE
+                );
                 AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
+                waitingAckFrom = SERVO_AX12_DONE;
             }
         break;
         case 104://ouvrir séparateur
-            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche
-                AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
-                AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
+            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+                AX12_setGoal(
+                    AX12_ID_BRAS_RELACHEUR_DROIT,
+                    AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
                 AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_RELACHEUR_DROIT;
+                waitingAckFrom = SERVO_AX12_DONE;
             } else {
-                AX12_setGoal(AX12_ID_BRAS_BASE,55,0x0FF);
-                AX12_setGoal(AX12_ID_BRAS_RELACHEUR,200);//fermer le bras
+                AX12_setGoal(
+                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
+                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
                 AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_RELACHEUR_GAUCHE;
+                waitingAckFrom = SERVO_AX12_DONE;
             }
         break;
         case 105://Rentrer le bras dans le robot, fermer le séparateur
-            //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
-            //AX12_processChange();
+            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+                AX12_setGoal(
+                    AX12_ID_BRAS_BASE_DROIT,
+                    AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
+                    AX12_SPEED_BRAS_BASE
+                );
+                AX12_setGoal(
+                    AX12_ID_BRAS_RELACHEUR_DROIT,
+                    AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
+                AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_BASE_DROIT;
+                waitingAckFrom = SERVO_AX12_DONE;
+            } else { 
+                AX12_setGoal(
+                    AX12_ID_BRAS_BASE_GAUCHE,
+                    AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
+                AX12_setGoal(
+                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
+                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
+                AX12_processChange();
+                waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
+                waitingAckFrom = SERVO_AX12_DONE;
+            }
         break;
-        case 106://descendre l'atrape coquillages  gauche si non inversé, droit si inversé
-            //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
-            //AX12_processChange();
+        case 106://descendre l'attrape coquillages au niveau des rochets  gauche si non inversé, droit si inversé
+            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+                AX12_setGoal(
+                    AX12_ID_PALET_DROIT,
+                    AX12_ANGLE_PALET_DROIT_ROCHET,
+                    AX12_SPEED_PALET
+                );
+                AX12_processChange();
+                waitingAckID = AX12_ID_PALET_DROIT;
+                waitingAckFrom = SERVO_AX12_DONE;
+            } else {
+                AX12_setGoal(
+                    AX12_ID_PALET_GAUCHE,
+                    AX12_ANGLE_PALET_GAUCHE_ROCHET,
+                    AX12_SPEED_PALET
+                );
+                AX12_processChange();
+                waitingAckID = AX12_ID_PALET_GAUCHE;
+                waitingAckFrom = SERVO_AX12_DONE;
+            }
+        break;
+        case 107://descendre l'attrape coquillages au niveau de la table  gauche si non inversé, droit si inversé
+            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+                AX12_setGoal(
+                    AX12_ID_PALET_DROIT,
+                    AX12_ANGLE_PALET_DROIT_OUVERT,
+                    AX12_SPEED_PALET
+                );
+                AX12_processChange();
+                waitingAckID = AX12_ID_PALET_DROIT;
+                waitingAckFrom = SERVO_AX12_DONE;
+            } else {
+                AX12_setGoal(
+                    AX12_ID_PALET_GAUCHE,
+                    AX12_ANGLE_PALET_GAUCHE_OUVERT,
+                    AX12_SPEED_PALET
+                );
+                AX12_processChange();
+                waitingAckID = AX12_ID_PALET_GAUCHE;
+                waitingAckFrom = SERVO_AX12_DONE;
+            }
+        break;
+        case 108://Remonter l'attrape coquillages   gauche si non inversé, droit si inversé
+            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+                AX12_setGoal(
+                    AX12_ID_PALET_DROIT,
+                    AX12_ANGLE_PALET_DROIT_FERMER,
+                    AX12_SPEED_PALET
+                );
+                AX12_processChange();
+                waitingAckID = AX12_ID_PALET_DROIT;
+                waitingAckFrom = SERVO_AX12_DONE;
+            } else {
+                AX12_setGoal(
+                    AX12_ID_PALET_GAUCHE,
+                    AX12_ANGLE_PALET_GAUCHE_FERMER,
+                    AX12_SPEED_PALET
+                );
+                AX12_processChange();
+                waitingAckID = AX12_ID_PALET_GAUCHE;
+                waitingAckFrom = SERVO_AX12_DONE;
+            }
         break;
         
+        
         case 200://ouvir la pince gauche
-            XL320_setGoal(XL320_ID_PINCE_GAUCHE, 350);
+            if(InversStrat == 1) {
+                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+            } else {
+                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+            }
         break;
         case 205://Sécuriser le palet gauche
-            XL320_setGoal(XL320_ID_PINCE_GAUCHE, 400);
+            if(InversStrat == 1) {
+                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
+            } else {
+                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
+            }
         break;
         case 202://fermer pince gauche
-            XL320_setGoal(XL320_ID_PINCE_GAUCHE, 650);
+            if(InversStrat == 1) {
+                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+            } else {
+                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+            }
         break;
         
         case 201://ouvir la pince droite
-            XL320_setGoal(XL320_ID_PINCE_DROITE, 400);
+            if(InversStrat == 1) {
+                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
+            } else {
+                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
+            }
         break;
         case 204://Sécuriser le palet droit
-            XL320_setGoal(XL320_ID_PINCE_DROITE, 300);
+            if(InversStrat == 1) {
+                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
+            } else {
+                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
+            }
         break;
         case 203://fermer pince droite
-            XL320_setGoal(XL320_ID_PINCE_DROITE, 70);
+            if(InversStrat == 1) {
+                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+            } else {
+                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+            }
         break;
         
         default:
             return 0;//L'action n'existe pas, il faut utiliser le CAN
         
     }
-    //return 1;//L'action est spécifique.
+    return 1;//L'action est spécifique.
     
 }
 
@@ -158,6 +315,12 @@
         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);
         default:
             strcpy(cheminFileStart,"/local/strat.txt");
             return 0;