Diff: Robots/Strategie_small.cpp

Revision:

10:a788d9cf60f2

Parent:

9:d0042422d95a

Child:

11:ed13a480ddca

--- a/Robots/Strategie_small.cpp	Thu Apr 28 08:11:36 2016 +0000
+++ b/Robots/Strategie_small.cpp	Fri Apr 29 09:14:27 2016 +0000
@@ -10,6 +10,23 @@
     
     
 }
+//L'angle est entre 0 et 1023
+void XL320_setGoal(unsigned char id, unsigned short angle);
+
+void XL320_setGoal(unsigned char id, unsigned short angle)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=SERVO_XL320;
+    msgTx.len=3;
+    msgTx.format=CANStandard;
+    msgTx.type=CANData;
+    msgTx.data[0]=(unsigned char)id;
+    // from sur 2 octets
+    msgTx.data[1]=(unsigned char)angle;
+    msgTx.data[2]=(unsigned char)(angle>>8);
+
+    can1.write(msgTx);
+}
 
 /****************************************************************************************/
 /* FUNCTION NAME: doAction                                                              */
@@ -17,7 +34,7 @@
 /****************************************************************************************/
 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
     switch(id) {
-        /*case 101://Descendre le bras pour les poissons
+        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);
                 AX12_processChange();
@@ -26,7 +43,7 @@
                 AX12_processChange();
             }
         break;
-        case 102://Remonter bras moiter
+        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);
                 AX12_processChange();
@@ -35,7 +52,7 @@
                 AX12_processChange();
             }
         break;
-        case 103://Lacher les poissons
+        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
                 AX12_processChange();
@@ -44,7 +61,7 @@
                 AX12_processChange();
             }
         break;
-        case 104://Rentrer le bras
+        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
@@ -55,18 +72,37 @@
                 AX12_processChange();
             }
         break;
-        case 105:
+        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();
+        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();
         break;
-        case 106:
-            //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
-            //AX12_processChange();
-        break;*/
+        
+        case 200://ouvir la pince gauche
+            XL320_setGoal(XL320_ID_PINCE_GAUCHE, 350);
+        break;
+        case 205://Sécuriser le palet gauche
+            XL320_setGoal(XL320_ID_PINCE_GAUCHE, 400);
+        break;
+        case 202://fermer pince gauche
+            XL320_setGoal(XL320_ID_PINCE_GAUCHE, 650);
+        break;
+        
+        case 201://ouvir la pince droite
+            XL320_setGoal(XL320_ID_PINCE_DROITE, 400);
+        break;
+        case 204://Sécuriser le palet droit
+            XL320_setGoal(XL320_ID_PINCE_DROITE, 300);
+        break;
+        case 203://fermer pince droite
+            XL320_setGoal(XL320_ID_PINCE_DROITE, 70);
+        break;
+        
         default:
-            waitingAckFrom = 0;
-            waitingAckID = 0;
-            return 1;//L'action n'existe pas, il faut utiliser le CAN
+            return 0;//L'action n'existe pas, il faut utiliser le CAN
         
     }
     //return 1;//L'action est spécifique.