Diff: Robots/Strategie_big.cpp

Revision:

12:14729d584500

Parent:

11:ed13a480ddca

Child:

14:c8fc06c4887f

--- a/Robots/Strategie_big.cpp	Mon May 02 19:40:59 2016 +0000
+++ b/Robots/Strategie_big.cpp	Mon May 09 09:10:17 2016 +0000
@@ -2,13 +2,14 @@
 #ifdef ROBOT_BIG
 #include "Config_big.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                      */
 /****************************************************************************************/
 void doFunnyAction(void) {
-    AX12_setGoal(AX12_ID_FUNNY_ACTION, 275);
-    AX12_setGoal(AX12_ID_FUNNY_ACTION, 180);
+    AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
     AX12_processChange();
     
 }
@@ -18,8 +19,46 @@
 /* DESCRIPTION  : Effectuer une action specifique                                       */
 /****************************************************************************************/
 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
+    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();
+        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();
+        break;
+        
+        case 102://Remonter le peigne
+            AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP);
+            AX12_processChange();
+        break;
+        case 103://Descendre le peigne
+            AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN);
+            AX12_processChange();
+        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();
+        break;
+        case 106://Remonter le support du cone arriere
+            AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE);
+            AX12_processChange();
+        break;
+        case 107://Descendre le support du cone arriere
+            AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE);
+            AX12_processChange();
+        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);
@@ -29,17 +68,21 @@
             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;*/
         break;
         
         case 112://Ouvrir la pince arrière basse
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 205);
-            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 95);
+            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;
         
         case 114://Ouvrir les portes arrières
@@ -48,10 +91,79 @@
             AX12_processChange();
         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, 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();
+        
+            
+            msgTx.id=POMPE_PWM;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=6;
+        
+            // x sur 2 octets
+            msgTx.data[0]=(unsigned char)POMPES_PWM;
+            msgTx.data[1]=(unsigned char)POMPES_PWM;
+            msgTx.data[2]=(unsigned char)POMPES_PWM;
+            msgTx.data[3]=(unsigned char)POMPES_PWM;
+            msgTx.data[4]=(unsigned char)POMPES_PWM;
+            msgTx.data[5]=(unsigned char)POMPES_PWM;
+        
+            can1.write(msgTx);
         break;
+        case 121://Désactiver les pompes
+            msgTx.id=POMPE_PWM;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=6;
+        
+            // x sur 2 octets
+            msgTx.data[0]=(unsigned char)0;
+            msgTx.data[1]=(unsigned char)0;
+            msgTx.data[2]=(unsigned char)0;
+            msgTx.data[3]=(unsigned char)0;
+            msgTx.data[4]=(unsigned char)0;
+            msgTx.data[5]=(unsigned char)0;
+        
+            can1.write(msgTx);
+        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;
+        
+        case 22://Changer la vitesse du robot
+            SendSpeed(speed,(unsigned short)angle);
+        break;
+        
+        case 30://Action tempo
+            wait_ms(speed);
+        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;
+        break;
+        
         default:
             return 0;//L'action n'existe pas, il faut utiliser le CAN
         
@@ -67,35 +179,61 @@
 void initRobot(void) 
 {
     //Enregistrement de tous les AX12 présent sur la carte
-    AX12_register(4,  AX12_SERIAL1);
-    AX12_register(14, AX12_SERIAL1);
-    AX12_register(15, AX12_SERIAL1);
     AX12_register(5,  AX12_SERIAL2);
     AX12_register(18, AX12_SERIAL2);
     AX12_register(13, AX12_SERIAL2);
-    AX12_register(1,  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(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
+    doAction(106,0,0);//Remonter le support du cone arriere
+}
+
+/****************************************************************************************/
 /* 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(500);
+    wait_ms(waitTime);
     doAction(110,0,0);//Ouverture pince arrière haute
-    wait_ms(500);
+    wait_ms(waitTime);
     doAction(113,0,0);//Fermeture pince arrière basse
-    wait_ms(500);
+    wait_ms(waitTime);
     doAction(112,0,0);//Ouverture pince arrière basse
-    wait_ms(500);
+    wait_ms(waitTime);
     doAction(115,0,0);//Fermeture porte arrière
-    wait_ms(500);
+    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
 }
 
 /****************************************************************************************/
@@ -116,10 +254,38 @@
         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");
+            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