code petit robot pour homologation

Fork of CRAC-Strat_2017_V2 by CRAC Team

Revision:
16:7321fb3bb396
Parent:
15:c2fc239e85df
Child:
18:cc5fec34ed9c
--- a/Robots/Strategie_small.cpp	Thu May 11 12:55:52 2017 +0000
+++ b/Robots/Strategie_small.cpp	Fri May 19 17:13:46 2017 +0000
@@ -83,29 +83,52 @@
         break;
         case 120:// poser une main tourneuse
             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
-                if(speed == 1) speed = 0;
-                else speed = 1;
-            } else {
-
+                if(speed == 2) speed = 1;
+                else speed = 2;
+                if(angle == 1) angle = 0;
+                else angle = 1;
             }
+            
+            if (speed == 1){
+                //Preparation_module_droit();
+            }else{
+                //Preparation_module_gauche();    
+            }
+            Preparation_module_gauche(); 
         break;
         
         case 121:// relever une main tourneuse
             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
-                if(speed == 1) speed = 0;
-                else speed = 1;
-            } else {
-
+                if(speed == 2) speed = 1;
+                else speed = 2;
+                if(angle == 1) angle = 0;
+                else angle = 1;
             }
+            
+            if (speed == 1){
+                //RangerBrasDroit();
+            }else{
+                RangerBrasGauche();    
+            }
+            
         break;
         
         case 122:// tourner un module
             if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
-                if(speed == 1) speed = 0;
-                else speed = 1;
-            } else {
-
+                if(speed == 2) speed = 1;
+                else speed = 2;
+                if(angle == 1) angle = 0;
+                else angle = 1;
             }
+            
+            /*if (speed == 1){
+                //Tourner_module_droite();
+            }else{
+                Tourner_module_gauche();    
+            }*/
+            Tourner_module_gauche();
+            
+            
         break;
         
         case 10://Désactiver le stop
@@ -119,6 +142,14 @@
         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;
         
         default:
@@ -152,6 +183,10 @@
     AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF);
     AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras
     AX12_processChange();*/
+    
+    initAX12();
+    moteurGauchePWM(0);
+    moteurDroitPWM(0);
 }
 
 /****************************************************************************************/
@@ -302,7 +337,6 @@
 int SelectStrategy(unsigned char id)
 {
     
-    
     switch(id)
     {
         case 1:
@@ -323,11 +357,29 @@
         case 6:
             strcpy(cheminFileStart,"/local/strat6.txt");
             return FileExists(cheminFileStart);
+        case 7:
+            strcpy(cheminFileStart,"/local/strat7.txt");
+            return FileExists(cheminFileStart);
+        case 8:
+            strcpy(cheminFileStart,"/local/strat8.txt");
+            return FileExists(cheminFileStart);
+        case 9:
+            strcpy(cheminFileStart,"/local/strat9.txt");
+            return FileExists(cheminFileStart);
         case 10:
+            strcpy(cheminFileStart,"/local/strat10.txt");
+            return FileExists(cheminFileStart);
+        case 11:
             strcpy(cheminFileStart,"/local/grand_8.txt");
             return FileExists(cheminFileStart);
+        
+        case 0x10:
+            strcpy(cheminFileStart,"/local/demoBras.txt");
+            return FileExists(cheminFileStart);
+        
         default:
             strcpy(cheminFileStart,"/local/strat1.txt");
+            SendRawId(0x258);
             return 0;
     }
 }