Diff: Robots/Strategie_big.cpp

Revision:

18:cc5fec34ed9c

Parent:

16:7321fb3bb396

Child:

19:b4b91258c275

--- a/Robots/Strategie_big.cpp	Fri May 19 17:14:07 2017 +0000
+++ b/Robots/Strategie_big.cpp	Mon May 22 15:01:49 2017 +0000
@@ -10,9 +10,16 @@
 /* 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();*/
-    
+    //envoie de la funny action
+    // 0x007, 01, 01
+        CANMessage msgTx=CANMessage();
+        msgTx.id=GLOBAL_FUNNY_ACTION;
+        msgTx.format=CANStandard;
+        msgTx.type=CANData;
+        msgTx.len=2;
+        msgTx.data[0]=0x01;
+        msgTx.data[1]=0x01;
+        can1.write(msgTx);
 }
 
 /****************************************************************************************/
@@ -21,171 +28,485 @@
 /****************************************************************************************/
 unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
     CANMessage msgTx=CANMessage();
+    int localData = 0, localData2;
     switch(id) {
+        case 100: // position initiale
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+                }
+            
+            msgTx.data[0]=1;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
         
-        case 100://preparation prise
+        case 101:  /// preparation prise
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+                }
+            
+            msgTx.data[0]=2;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
+        
+        case 102: // stockage haut
             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;
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
                 }
+            
+            msgTx.data[0]=3;
             msgTx.data[1]=speed;
             can1.write(msgTx);
         break;
-        case 101://stockage haut
+        
+        case 103: // stockage bas
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+                }
+            
+            msgTx.data[0]=4;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
+        
+        case 104: // pousser module
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+                }
+            
+            msgTx.data[0]=7;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
+        
+        case 105: // preparation prise
             msgTx.id=SERVO_AX12_ACTION;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=2;
-        
-            // action et le cote selectionné
-            msgTx.data[0]=2;
+            
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+                }
+            
+            msgTx.data[0]=6;
             msgTx.data[1]=speed;
-        
             can1.write(msgTx);
         break;
         
-        case 102://stockage bas
+        case 106: //deposer module
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+                }
+            
+            msgTx.data[0]=5;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
+        
+        case 110: // repos bras avant
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            speed = BRAS_AVANT; 
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+            }
+            if (speed == BRAS_AVANT)localData = 0x10;
+            else localData = 0x24;
+            
+            msgTx.data[0]=localData;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
+        
+        case 111: // init bras module
             msgTx.id=SERVO_AX12_ACTION;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=2;
-        
-            // action et le cote selectionné
-            msgTx.data[0]=3;
+            speed = BRAS_AVANT;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+            }
+            if (speed == BRAS_AVANT)localData = 0x0A;
+            else localData = 0x1E;
+            
+            msgTx.data[0]=localData;
             msgTx.data[1]=speed;
-        
             can1.write(msgTx);
         break;
-        case 103://deposer
+        
+        case 112: // bouger module avant la gouttière
             msgTx.id=SERVO_AX12_ACTION;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=2;
-        
-            // action et le cote selectionné
-            msgTx.data[0]=4;
+            speed = BRAS_AVANT;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                    if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                    else speed = BRAS_AVANT;
+            }
+            if (speed == BRAS_AVANT)localData = 0x0B;
+            else localData = 0x1F;
+            
+            msgTx.data[0]=localData;
             msgTx.data[1]=speed;
-        
             can1.write(msgTx);
         break;
         
-        case 104://preparation depot bas
+        case 113: // deposer module
             msgTx.id=SERVO_AX12_ACTION;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=2;
-        
-            // action et le cote selectionné
-            msgTx.data[0]=5;
+            localData2 = BRAS_AVANT;
+            
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
+                else localData2 = BRAS_AVANT;
+            }
+            
+            if(localData2 == BRAS_AVANT){
+                if(speed == INV){
+                        localData = 0x0E;
+                    }else{
+                        localData = 0x0C;
+                    }
+            }else{
+                if(speed == INV){
+                        localData = 0x22;
+                    }else{
+                        localData = 0x20;
+                    }
+            }
+            
+            msgTx.data[0]=localData;
             msgTx.data[1]=speed;
-        
             can1.write(msgTx);
         break;
-        case 105://preparation depot haut
+        
+        case 114: // repositionner bras avant
             msgTx.id=SERVO_AX12_ACTION;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=2;
+            speed = BRAS_AVANT;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                else speed = BRAS_AVANT;
+            }
+            if (speed == BRAS_AVANT)localData = 0x0F;
+            else localData = 0x23; 
+            
+            msgTx.data[0]=localData;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
         
-            // action et le cote selectionné
-            msgTx.data[0]=6;
-            msgTx.data[1]=speed;
+        case 115: // pompe avant ON
+            msgTx.id=0x400;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            localData = BRAS_AVANT;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
+                else localData = BRAS_AVANT;
+            } 
+            
+            msgTx.data[0]=1;
+            msgTx.data[1]=localData;
+            
+            can1.write(msgTx);
+            wait_ms(1);
+            can1.write(msgTx);
+        break;
         
+        case 116: // pompe avant OFF
+            msgTx.id=0x400;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            localData = BRAS_AVANT;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
+                else localData = BRAS_AVANT;
+            } 
+            
+            msgTx.data[0]=0;
+            msgTx.data[1]=localData;
+            
+            can1.write(msgTx);
+            wait_ms(1);
             can1.write(msgTx);
         break;
-        case 106://pousser module
+        
+        case 120: // repos bras arrière
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            speed = BRAS_ARRIERE;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                else speed = BRAS_AVANT;
+            }
+            if (speed == BRAS_AVANT)localData = 0x10;
+            else localData = 0x24; 
+            
+            msgTx.data[0]=localData;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
+        
+        case 121: // position d'init avant de prendre un module bras arriere
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            speed = BRAS_ARRIERE;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                else speed = BRAS_AVANT;
+            }
+            if (speed == BRAS_AVANT)localData = 0xA0;
+            else localData = 0x1E; 
+            
+            msgTx.data[0]=localData;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
+        
+        case 122: // bouger le bras arriere avant la gouttiere
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            speed = BRAS_ARRIERE;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                else speed = BRAS_AVANT;
+            }
+            if (speed == BRAS_AVANT)localData = 0x0B;
+            else localData = 0x1F; 
+            
+            msgTx.data[0]=localData;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
+        
+        case 123: // deposer module
             msgTx.id=SERVO_AX12_ACTION;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=2;
-        
-            // action et le cote selectionné
-            msgTx.data[0]=7;
+            localData2 = BRAS_ARRIERE;
+            
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
+                else localData2 = BRAS_AVANT;
+            }
+            
+            if(localData2 == BRAS_AVANT){
+                if(speed == INV){
+                        localData = 0x0E;
+                    }else{
+                        localData = 0x0C;
+                    }
+            }else{
+                if(speed == INV){
+                        localData = 0x22;
+                    }else{
+                        localData = 0x20;
+                    }
+            }
+            
+            msgTx.data[0]=localData;
             msgTx.data[1]=speed;
-        
             can1.write(msgTx);
         break;
         
-        case 110://Ouvrir la pince arrière haute
-           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
-            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
+        case 124: // remettre le bras arriere en posiion initiale
             msgTx.id=SERVO_AX12_ACTION;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=2;
+            speed = BRAS_ARRIERE;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+                else speed = BRAS_AVANT;
+            }
+            if (speed == BRAS_AVANT)localData = 0x0F;
+            else localData = 0x23; 
+            
+            msgTx.data[0]=localData;
+            msgTx.data[1]=speed;
+            can1.write(msgTx);
+        break;
         
-            // action et le cote selectionné
-            msgTx.data[0]=12;
-            msgTx.data[1]=speed;
+        case 125: // pompe pwm arriere ON
+            msgTx.id=0x400;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            localData = BRAS_ARRIERE;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
+                else localData = BRAS_AVANT;
+            } 
+            
+            msgTx.data[0]=1;
+            msgTx.data[1]=localData;
+            
+            can1.write(msgTx);
+            wait_ms(1);
+            can1.write(msgTx);
+        break;
+        
+        case 126: // pompe arriere pwm OFF
+            msgTx.id=0x400;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            localData = BRAS_ARRIERE;
+            if (InversStrat){ // si on est inversé, on echange les bras
+                if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
+                else localData = BRAS_AVANT;
+            } 
+            
+            msgTx.data[0]=0;
+            msgTx.data[1]=localData;
+            
+            can1.write(msgTx);
+            wait_ms(1);
+            can1.write(msgTx);
+        break;
+        
+        case 130: // baiser bras turbine
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            msgTx.data[0]=0x14;
+            msgTx.data[1]=BRAS_AVANT;
+            can1.write(msgTx);
+        break;
         
+        case 131: // lever bras turbine
+            msgTx.id=SERVO_AX12_ACTION;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            msgTx.data[0]=0x15;
+            msgTx.data[1]=BRAS_AVANT;
+            can1.write(msgTx);
+        break;
+        
+        case 132: // allumer turbine
+            msgTx.id=0x403;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            msgTx.data[0]=1;
+            msgTx.data[1]=BRAS_AVANT;
+            can1.write(msgTx);
+        break;
+        
+        case 133: // stop turbine
+            msgTx.id=0x403;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            msgTx.data[0]=0;
+            msgTx.data[1]=BRAS_AVANT;
+            can1.write(msgTx);
+        break;
+        
+        case 140:// lanceur ON
+            msgTx.id=0x402;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            
+            msgTx.data[0]=1;
+            msgTx.data[1]=0x02;
+            can1.write(msgTx);
+        break;
+        
+        case 141: // lanceur OFF
+            msgTx.id=0x402;
+            msgTx.format=CANStandard;
+            msgTx.type=CANData;
+            msgTx.len=2;
+            
+            msgTx.data[0]=0;
+            msgTx.data[1]=0x02;
             can1.write(msgTx);
         break;
 
-        case 120://Activer les pompes
-            /*AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
-            AX12_processChange();*/
-        
-            
-            msgTx.id=POMPE_PWM;
+        case 150:
+            msgTx.id=0x404;
             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;
-        
+            msgTx.len=2;
+            
+            msgTx.data[0]=1;
+            msgTx.data[1]=BRAS_AVANT;
+            
             can1.write(msgTx);
         break;
-        case 121://Désactiver les pompes
-            msgTx.id=POMPE_PWM;
+        
+        case 151:
+            msgTx.id=0x404;
             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;
-        
+            msgTx.len=2;
+            
+            msgTx.data[0]=0;
+            msgTx.data[1]=BRAS_AVANT;
+            
             can1.write(msgTx);
         break;
         
@@ -210,7 +531,7 @@
             wait_ms(speed);
         break;
         
-        case 40: // demande au telemetre la position d'un objet
+        /*case 40: // demande au telemetre la position d'un objet
             //SendRawId(TELEMETRE_RECHERCHE_OBJET);
             
             modeTelemetre = 1;
@@ -224,7 +545,7 @@
             // 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);
@@ -235,20 +556,20 @@
             //msgTx.data[4]=(unsigned char)theta;
             //msgTx.data[5]=(unsigned char)(theta>>8);
             msgTx.data[4]=0;
-            msgTx.data[5]=0;*/
+            msgTx.data[5]=0;
             
             can1.write(msgTx);
             
-        break;
+        break;*/
         
-        case 130://Lancer mouvement de sortie de la zone de départ
+       /* case 130://Lancer mouvement de sortie de la zone de départ
             msgTx.id=ACTION_BIG_DEMARRAGE;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=1;
             msgTx.data[0] = (unsigned char)speed;
             can1.write(msgTx);
-        break;
+        break;*/
         
         default:
             return 0;//L'action n'existe pas, il faut utiliser le CAN
@@ -285,12 +606,14 @@
 /****************************************************************************************/
 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
+    wait(2);
+    doAction(101,1,0);// preparation prise pince arriere
+    doAction(101,0,0);// preparation prise pince avant
+    doAction(130,0,0);// baisser aspitr 
+    wait(2);
+    doAction(100,1,0);// preparation prise pince arriere
+    doAction(100,0,0);// preparation prise pince avant
+    doAction(131,0,0);// baisser aspitr  
 }
 
 /****************************************************************************************/
@@ -299,6 +622,7 @@
 /****************************************************************************************/
 void runRobotTest(void) 
 {
+    /*
     int waitTime = 500;
     
     //Test des AX12 dans l'ordre
@@ -320,7 +644,7 @@
     wait_ms(waitTime);
     doAction(103,0,0);//Descendre le peigne
     wait_ms(waitTime);
-    doAction(102,0,0);//Remonter le peigne
+    doAction(102,0,0);//Remonter le peigne*/
 }
 
 /****************************************************************************************/