CRAC Team / Mbed 2 deprecated carte_straegie_2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Diff: Robots/Strategie_small.cpp

Revision:
12:14729d584500
Parent:
11:ed13a480ddca
Child:
15:c2fc239e85df
--- a/Robots/Strategie_small.cpp	Mon May 02 19:40:59 2016 +0000
+++ b/Robots/Strategie_small.cpp	Mon May 09 09:10:17 2016 +0000
@@ -2,6 +2,8 @@
 #ifdef ROBOT_SMALL
 #include "Config_small.h"
 
+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                      */
@@ -41,6 +43,11 @@
                     AX12_ANGLE_BRAS_BASE_DROIT_OUVERT,
                     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;
@@ -50,6 +57,11 @@
                     AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT,
                     AX12_SPEED_BRAS_BASE
                 );
+                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;
@@ -121,27 +133,31 @@
         case 105://Rentrer le bras dans le robot, fermer le séparateur
             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_REPLIER,
+                    AX12_SPEED_BRAS_RELACHEUR
+                );
+                AX12_processChange();
+                wait_ms(100);
+                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_ID_BRAS_RELACHEUR_GAUCHE,
+                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
                     AX12_SPEED_BRAS_RELACHEUR
                 );
+                AX12_processChange();
+                wait_ms(100);
                 AX12_setGoal(
-                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
-                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+                    AX12_ID_BRAS_BASE_GAUCHE,
+                    AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
                     AX12_SPEED_BRAS_RELACHEUR
                 );
                 AX12_processChange();
@@ -216,9 +232,9 @@
         
         case 200://ouvir la pince gauche
             if(InversStrat == 1) {
-                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
             } else {
-                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
             }
         break;
         case 205://Sécuriser le palet gauche
@@ -257,6 +273,18 @@
                 XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
             }
         break;
+        case 10://Désactiver le stop
+            isStopEnable = 0;
+        break;
+        case 11://Activer le stop
+            isStopEnable = 1;
+        break;
+        case 20://Désactiver l'asservissement
+            setAsservissementEtat(0);
+        break;
+        case 21://Activer l'asservissement
+            setAsservissementEtat(1);
+        break;
         
         default:
             return 0;//L'action n'existe pas, il faut utiliser le CAN
@@ -280,6 +308,9 @@
     AX12_register(4,AX12_SERIAL2);
     AX12_register(16,AX12_SERIAL2);
     AX12_register(17,AX12_SERIAL2,0x0FF);
+    
+    //runRobotTest();
+    
     /*
     AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
     AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
@@ -289,12 +320,143 @@
 }
 
 /****************************************************************************************/
+/* FUNCTION NAME: initRobotActionneur                                                   */
+/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
+/****************************************************************************************/
+void initRobotActionneur(void)
+{
+    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_DROIT,
+        AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_GAUCHE,
+        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_DROIT,
+        AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
+        AX12_SPEED_BRAS_BASE
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_GAUCHE,
+        AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_PALET_DROIT,
+        AX12_ANGLE_PALET_DROIT_FERMER,
+        AX12_SPEED_PALET
+    );
+    AX12_setGoal(
+        AX12_ID_PALET_GAUCHE,
+        AX12_ANGLE_PALET_GAUCHE_FERMER,
+        AX12_SPEED_PALET
+    );
+    AX12_processChange();
+}
+
+/****************************************************************************************/
 /* FUNCTION NAME: runTest                                                               */
 /* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
 /****************************************************************************************/
 void runRobotTest(void) 
 {
+    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
+    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
     
+    wait_ms(200);
+    
+    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+    
+    wait_ms(200);
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_DROIT,
+        AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
+        AX12_SPEED_BRAS_BASE
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_GAUCHE,
+        AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
+        AX12_SPEED_BRAS_BASE
+    );
+    AX12_processChange();
+        
+    wait_ms(500);        
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_DROIT,
+        AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_GAUCHE,
+        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_processChange();
+    
+    wait_ms(600);
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_DROIT,
+        AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_RELACHEUR_GAUCHE,
+        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_processChange();
+    
+    wait_ms(500);
+    
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_DROIT,
+        AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
+        AX12_SPEED_BRAS_BASE
+    );
+    AX12_setGoal(
+        AX12_ID_BRAS_BASE_GAUCHE,
+        AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
+        AX12_SPEED_BRAS_RELACHEUR
+    );
+    AX12_processChange();
+    
+    wait_ms(600);
+    AX12_setGoal(
+        AX12_ID_PALET_DROIT,
+        AX12_ANGLE_PALET_DROIT_ROCHET,
+        AX12_SPEED_PALET
+    );
+    AX12_setGoal(
+        AX12_ID_PALET_GAUCHE,
+        AX12_ANGLE_PALET_GAUCHE_ROCHET,
+        AX12_SPEED_PALET
+    );
+    AX12_processChange();
+    
+    wait_ms(500);
+    
+    AX12_setGoal(
+        AX12_ID_PALET_DROIT,
+        AX12_ANGLE_PALET_DROIT_FERMER,
+        AX12_SPEED_PALET
+    );
+    AX12_setGoal(
+        AX12_ID_PALET_GAUCHE,
+        AX12_ANGLE_PALET_GAUCHE_FERMER,
+        AX12_SPEED_PALET
+    );
+    AX12_processChange();
 }
 
 /****************************************************************************************/
@@ -304,6 +466,8 @@
 /****************************************************************************************/
 int SelectStrategy(unsigned char id)
 {
+    
+    
     switch(id)
     {
         case 1:
@@ -321,10 +485,34 @@
         case 5:
             strcpy(cheminFileStart,"/local/strat5.txt");
             return FileExists(cheminFileStart);
+        case 6:
+            strcpy(cheminFileStart,"/local/strat6.txt");
+            return FileExists(cheminFileStart);
+        case 10:
+            strcpy(cheminFileStart,"/local/grand_8.txt");
+            return FileExists(cheminFileStart);
         default:
-            strcpy(cheminFileStart,"/local/strat.txt");
+            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)
+{
+    
+}
+
 #endif