Diff: Actionneur.cpp

Revision:

3:0175a071dd65

Parent:

2:9d280856a536

Child:

4:5cf096b1c65d

--- a/Actionneur.cpp	Fri May 19 08:06:49 2017 +0000
+++ b/Actionneur.cpp	Sat May 20 09:36:35 2017 +0000
@@ -1,4 +1,4 @@
-#include "Actionneur.h"
+#include "all_includes.h"
 
 extern CAN can;       
         
@@ -9,17 +9,13 @@
 
 extern DigitalOut led2;
 extern Serial pc;
+extern Timer t;
 extern void GetPositionAx12(void);
-extern void gerer_turbine(unsigned char pwm_turbine);
-extern PwmOut PompeDroite;
-extern unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
-                                unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
-                                unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3);
 
-static float TAB_ANGLE1[4], TAB_ANGLE2[4];
-static char TAB_POSITION[4];
-AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; 
-AX12 *MilieuBrasPompe, *HautBrasPompe, *BasBrasPompe, *BrasPompeAx12, *TabPompeBras;                                 
+AX12 *BaseBrasCentralPR, *CoudeBrasCentralPR, *PinceDBrasCentralPR, *PinceGBrasCentralPR, *DoigtBrasCentralPR, *BrasCentralPRAx12, *TabBrasCentralPR,
+     *CrocBrasGauchePR, *EpauleBrasGauchePR, *CoudeBrasGauchePR, *BrasGauchePRAx12, *TabBrasGauchePR,
+     *CrocBrasDroitPR, *EpauleBrasDroitPR, *CoudeBrasDroitPR, *BrasDroitPRAx12, *TabBrasDroitPR; 
+                             
                                  
                      
                    
@@ -56,30 +52,70 @@
 {
     short vitesse=700;
     
-    HautBrasPompe = new AX12(p9, p10, 3, 1000000);  
-    MilieuBrasPompe = new AX12(p9, p10, 2, 1000000); 
-    BasBrasPompe = new AX12(p9, p10, 1, 1000000);
+    BaseBrasCentralPR = new AX12(p9, p10, 5, 1000000);  
+    CoudeBrasCentralPR = new AX12(p9, p10, 6, 1000000); 
+    PinceDBrasCentralPR = new AX12(p9, p10, 8, 1000000);
+    PinceGBrasCentralPR = new AX12(p9, p10, 7, 1000000);
+    DoigtBrasCentralPR = new AX12(p9, p10, 4, 1000000);
     
-    BrasPompeAx12 = new AX12(p9,p10,0xFE,1000000);
+    CrocBrasGauchePR = new AX12(p13, p14, 3, 1000000);
+    CoudeBrasGauchePR = new AX12(p13, p14, 2, 1000000);
+    EpauleBrasGauchePR = new AX12(p13, p14, 1, 1000000);
+    
+    CrocBrasDroitPR = new AX12(p28, p27, 11, 1000000);
+    CoudeBrasDroitPR = new AX12(p28, p27, 10, 1000000);
+    EpauleBrasDroitPR = new AX12(p28, p27, 9, 1000000);
+    
+    BrasCentralPRAx12 = new AX12(p9,p10,0xFE,1000000);
+    BrasGauchePRAx12 = new AX12(p13,p14,0xFE,1000000);
+    BrasDroitPRAx12 = new AX12(p28,p27,0xFE,1000000);
     
-    HautBrasPompe->Set_Goal_speed(vitesse);  
-    MilieuBrasPompe->Set_Goal_speed(vitesse); 
-    BasBrasPompe->Set_Goal_speed(vitesse); 
+    BaseBrasCentralPR->Set_Goal_speed(vitesse);  
+    CoudeBrasCentralPR->Set_Goal_speed(vitesse); 
+    PinceDBrasCentralPR->Set_Goal_speed(vitesse); 
+    PinceGBrasCentralPR->Set_Goal_speed(vitesse); 
+    DoigtBrasCentralPR->Set_Goal_speed(vitesse);
+    
+    CrocBrasGauchePR->Set_Goal_speed(vitesse); 
+    CoudeBrasGauchePR->Set_Goal_speed(vitesse); 
+    EpauleBrasGauchePR->Set_Goal_speed(vitesse);
+    
+    CrocBrasDroitPR->Set_Goal_speed(vitesse); 
+    CoudeBrasDroitPR->Set_Goal_speed(vitesse); 
+    EpauleBrasDroitPR->Set_Goal_speed(vitesse);
     
-    HautBrasPompe->Set_Mode(0); 
-    MilieuBrasPompe->Set_Mode(0); 
-    BasBrasPompe->Set_Mode(0); 
-  
+    BaseBrasCentralPR->Set_Mode(0); 
+    CoudeBrasCentralPR->Set_Mode(0); 
+    PinceDBrasCentralPR->Set_Mode(0); 
+    PinceGBrasCentralPR->Set_Mode(0); 
+    DoigtBrasCentralPR->Set_Mode(0);
+    
+    CrocBrasGauchePR->Set_Mode(0); 
+    CoudeBrasGauchePR->Set_Mode(0);
+    EpauleBrasGauchePR->Set_Mode(0);
+    
+    CrocBrasDroitPR->Set_Mode(0); 
+    CoudeBrasDroitPR->Set_Mode(0);
+    EpauleBrasDroitPR->Set_Mode(0);
 } 
 
 void GetPositionAx12(void) {
-    
-    pc.printf("\n\r * Bras Pompe * \n\r");
+
+    pc.printf("\n\r ");
+
+    pc.printf("BaseC  : %lf \n\r ",   BaseBrasCentralPR->Get_Position()   );
+    pc.printf("CoudeC : %lf \n\r ",   CoudeBrasCentralPR->Get_Position()  );
+    pc.printf("PinceCD : %lf \n\r ",   PinceDBrasCentralPR->Get_Position());   
+    pc.printf("PinceCG : %lf \n\r ",   PinceGBrasCentralPR->Get_Position()); 
+    pc.printf("DoigtC : %lf \n\r ",   DoigtBrasCentralPR->Get_Position()  );    
     
-    pc.printf("Haut  : %lf \n\r ",   HautBrasPompe->Get_Position()      );
-    pc.printf("Milieu : %lf \n\r ",   MilieuBrasPompe->Get_Position()    );
-    pc.printf("Bas: %lf \n\r ",   BasBrasPompe->Get_Position() );   
+    pc.printf("EpauleG : %lf \n\r ",   EpauleBrasGauchePR->Get_Position());   
+    pc.printf("CoudeG : %lf \n\r ",   CoudeBrasGauchePR->Get_Position() ); 
+    pc.printf("CrocG : %lf \n\r ",   CrocBrasGauchePR->Get_Position()   ); 
     
+    pc.printf("EpauleD : %lf \n\r ",   EpauleBrasDroitPR->Get_Position());   
+    pc.printf("CoudeD : %lf \n\r ",   CoudeBrasDroitPR->Get_Position() ); 
+    pc.printf("CrocD : %lf \n\r ",   CrocBrasDroitPR->Get_Position()   ); 
 }
 
 
@@ -87,391 +123,390 @@
 /* FUNCTION NAME: Automate_ax12                                                         */
 /* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
 /****************************************************************************************/
-void AX12_automate(unsigned char etat_ax12, unsigned char choix_bras){
+void AX12_automate(unsigned char etat_ax12){
     
     unsigned short speed;
-    
-    static unsigned char action = 0;
-    unsigned int GoalPos1, GoalPos2, GoalPos3;
+    unsigned int GoalPosDoigt, GoalPosBase, GoalPosCoude, GoalPosPinceG, GoalPosPinceD,
+                 GoalPosEpauleTournanteG, GoalPosCoudeTournanteG,
+                 GoalPosEpauleTournanteD, GoalPosCoudeTournanteD;
+        
+    speed = 1000;    
+        
     switch(etat_ax12){
+               
+        case AX12_PINCE_CENTRALE_POSITION_INITIALE :
+            wait_ms(TIME);
+            speed = 511;
+            GoalPosDoigt=1150;
+            GoalPosBase=1490;
+            GoalPosCoude=1470;
+            GoalPosPinceG=1090;
+            GoalPosPinceD=1930;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            Fin_action();  
+            break;
+            
+        case AX12_PINCE_CENTRALE_PREPARATION_PRISE :
+            wait_ms(TIME);
+            GoalPosDoigt=90;
+            GoalPosBase=170;
+            GoalPosCoude=1000;
+            GoalPosPinceG=1090;
+            GoalPosPinceD=1930;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            Fin_action();  
+            break;    
+            
+        case AX12_PINCE_CENTRALE_PRISE_MODULE :
+            wait_ms(TIME);
+            GoalPosDoigt=90;
+            GoalPosBase=170;
+            GoalPosCoude=1000;
+            GoalPosPinceG=500;
+            GoalPosPinceD=2500;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            Fin_action();  
+            break;  
+            
+        case AX12_PINCE_CENTRALE_STOCKAGE_HAUT :
+            wait_ms(TIME);
+            GoalPosDoigt=90;
+            GoalPosBase=1300;
+            GoalPosCoude=700;
+            GoalPosPinceG=500;
+            GoalPosPinceD=2500;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            
+            wait(TIME*5);
+            GoalPosDoigt=90;
+            GoalPosBase=1300;
+            GoalPosCoude=1200;
+            GoalPosPinceG=500;
+            GoalPosPinceD=2500;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            
+            wait(TIME*5);
+            GoalPosDoigt=90;
+            GoalPosBase=1300;
+            GoalPosCoude=1200;
+            GoalPosPinceG=1090;
+            GoalPosPinceD=1930;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            Fin_action();  
+            break;  
         
-        case AX12_GET_POSITION:
-            GetPositionAx12();
+        case AX12_PINCE_CENTRALE_STOCKAGE_BAS :
+            wait_ms(TIME);
+            GoalPosDoigt=90;
+            GoalPosBase=1200;
+            GoalPosCoude=443;
+            GoalPosPinceG=500;
+            GoalPosPinceD=2500;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            Fin_action();  
             break;
         
-        
-        case AX12_VIDER_FUSEE_POMPE:
-            
-            speed=511;
-            
-            GoalPos1=1500;
-            GoalPos2=624;
-            GoalPos3=1413;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-            GoalPos1+=900;
-            GoalPos2=202;
-            GoalPos3=2205;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(20*TIME);
-            GoalPos3=2557;
-            GoalPos2=173;
-            GoalPos1=1947;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
-            GoalPos3=2557;
-            GoalPos2=370;
-            GoalPos1=1798;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-            GoalPos3=2548;
-            GoalPos2=683;
-            GoalPos1=1499;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-            GoalPos3=2405;
-            GoalPos2=1150;
-            GoalPos1=1158;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-              
-            PompeDroite.write(0);
+        case AX12_PINCE_CENTRALE_PREPARATION_DEPOT :
+            wait_ms(TIME);
+            GoalPosDoigt=90;
+            GoalPosBase=639;
+            GoalPosCoude=557;
+            GoalPosPinceG=400;
+            GoalPosPinceD=2500;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
 
-            Fin_action();          
-            break;
-            
-        case AX12_VIDER_FUSEE_POMPEBLEU:
-            
-            speed=511;
-            
-            GoalPos3=2205;
-            GoalPos2=202;
-            GoalPos1=598;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-            GoalPos3=2557;
-            GoalPos2=173;
-            GoalPos1=1050;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(5*TIME);
-            GoalPos3=2557;
-            GoalPos2=370;
-            GoalPos1=1199;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
-            GoalPos3=2548;
-            GoalPos2=683;
-            GoalPos1=1499;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-            GoalPos3=2405;
-            GoalPos2=1150;
-            GoalPos1=1947;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
-            
-            PompeDroite.write(0);
-
-            Fin_action();          
+            wait(TIME*10);
+            GoalPosDoigt=90;
+            GoalPosBase=400;
+            GoalPosCoude=400;
+            GoalPosPinceG=500;
+            GoalPosPinceD=2500;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+    
+            Fin_action();  
             break;
             
-         case AX12_REMPLIR_BASE_POMPE:
-         
-            speed=511;
+        case AX12_PINCE_CENTRALE_DEPOSER :
+            //DEPOSER
+            wait_ms(TIME);
+            GoalPosDoigt=90;
+            GoalPosBase=440;
+            GoalPosCoude=440;
+            GoalPosPinceG=1090;
+            GoalPosPinceD=1930;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+
+            Fin_action();  
+            break;
+        
+        case AX12_PINCE_CENTRALE_DEPOT_HAUT :
+            wait(TIME*5);
+            GoalPosDoigt=90;
+            GoalPosBase=1300;
+            GoalPosCoude=1200;
+            GoalPosPinceG=1090;
+            GoalPosPinceD=1930;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            
+            wait(TIME*10);
+            GoalPosDoigt=90;
+            GoalPosBase=1300;
+            GoalPosCoude=1200;
+            GoalPosPinceG=500;
+            GoalPosPinceD=2500;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
             
-            GoalPos3=2440;
-            GoalPos2=302;
-            GoalPos1=1950;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-            GoalPos3=1865;
-            GoalPos2=252;
-            GoalPos1=2400;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-            GoalPos3=1500;
-            GoalPos2=460;
-            GoalPos1=2400;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
-            GoalPos3=1396;
-            GoalPos2=600;
-            GoalPos1=1399;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
+            wait_ms(TIME);
+            GoalPosDoigt=90;
+            GoalPosBase=1300;
+            GoalPosCoude=700;
+            GoalPosPinceG=500;
+            GoalPosPinceD=2500;
+            mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);   
+            
+            Fin_action();   
+            break;  
+            
+        case AX12_GAUCHE_CROC_OUVERT :
+            wait_ms(TIME);
+            CrocBrasGauchePR->Set_Secure_Goal(200);
+            Fin_action();  
+            break;
             
-            GoalPos3=1413;
-            GoalPos2=624;
-            GoalPos1=1500;
-            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
+        case AX12_GAUCHE_CROC_FERME :
+            wait_ms(TIME);
+            CrocBrasGauchePR->Set_Secure_Goal(240);
+            Fin_action();  
+            break;     
             
-            Fin_action();
-            break;   
-        
-        case AX12_INITIALISATION :
-            if (FlagAx12 == 1){
-                Initialisation_position(1);
-                //Check_positionAX12(&TAB1[25], 1);
-                Initialisation_position(2);
-                //Check_positionAX12(&TAB21[25], 2);
-                FlagAx12 = 2;
-            }
-            else if (choix_bras > 1){
-                Initialisation_position(choix_bras);
-            }
+        case AX12_GAUCHE_CROC_INITIALE :
+            wait_ms(TIME);
+            CrocBrasGauchePR->Set_Secure_Goal(300);
+            Fin_action();  
+            break;         
+            
+        case AX12_TOURNANTE_GAUCHE_POSITION_INITIALE :
+            wait_ms(TIME);
+            speed = 511;
+            GoalPosCoudeTournanteG=1450;
+            GoalPosEpauleTournanteG=600;
+            mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   
+
+            Fin_action();  
             break;
-                        
-        case AX12_PREPARATION_PRISE :
-            Preparation_prise(choix_bras);
-            if (action == 0){
-                Fin_action();
-                action ++;
-            }
+            
+        case AX12_TOURNANTE_GAUCHE_PREPARATION :
+            wait_ms(TIME);
+            speed = 511;
+            GoalPosCoudeTournanteG=930;
+            GoalPosEpauleTournanteG=1962;
+            mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   
+
+            Fin_action();  
             break;
-                        
-        case AX12_STOCKAGE_HAUT :
-            Stockage_haut(choix_bras);
-            etat_ax12 = AX12_DEFAUT;
-            Fin_action();
+            
+        case AX12_TOURNANTE_GAUCHE_MODULE :
+            wait_ms(TIME);
+            speed = 511;
+            GoalPosCoudeTournanteG=894;
+            GoalPosEpauleTournanteG=2200;
+            mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);   
+
+            Fin_action();  
             break;
-                        
-        case AX12_STOCKAGE_BAS :
-            Stockage_bas(choix_bras);
-            etat_ax12 = AX12_DEFAUT;
-            Fin_action();
+            
+        case AX12_DROIT_CROC_OUVERT :
+            wait_ms(TIME);
+            CrocBrasDroitPR->Set_Secure_Goal(106);
+            Fin_action();  
             break;
-                        
-        case AX12_DEPOSER :
-            Deposer(choix_bras);
-            etat_ax12 = AX12_DEFAUT;
-            Fin_action();
-            break;
-                        
-        case AX12_PREPARATION_DEPOT_BAS :
-            Preparation_depot_bas(choix_bras);
-            etat_ax12 = AX12_DEFAUT;
-            Fin_action();
+            
+        case AX12_DROIT_CROC_FERME :
+            wait_ms(TIME);
+            CrocBrasDroitPR->Set_Secure_Goal(55);
+            Fin_action();  
+            break;    
+            
+        case AX12_DROIT_CROC_INITIALE :
+            wait_ms(TIME);
+            CrocBrasDroitPR->Set_Secure_Goal(0);
+            Fin_action();  
+            break;     
+            
+        case AX12_TOURNANTE_DROIT_POSITION_INITIALE :
+            wait_ms(TIME);
+            speed = 511;
+            GoalPosCoudeTournanteD=1450;
+            GoalPosEpauleTournanteD=600;
+            mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   
+
+            Fin_action();  
             break;
-                        
-        case AX12_PREPARATION_DEPOT_HAUT :
-            Preparation_depot_haut(choix_bras);
-            etat_ax12 = AX12_DEFAUT;
-            Fin_action();
+            
+        case AX12_TOURNANTE_DROIT_PREPARATION :
+            wait_ms(TIME);
+            speed = 511;
+            GoalPosCoudeTournanteD=930;
+            GoalPosEpauleTournanteD=1962;
+            mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   
+
+            Fin_action();  
             break;
-                        
-        case AX12_POUSSER_MODULE :
-            Pousser_module(choix_bras);
-            etat_ax12 = AX12_DEFAUT;
-            Fin_action();
+            
+        case AX12_TOURNANTE_DROIT_MODULE :
+            wait_ms(TIME);
+            speed = 511;
+            GoalPosCoudeTournanteD=894;
+            GoalPosEpauleTournanteD=2200;
+            mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);   
+
+            Fin_action();  
             break;
         
         case AX12_DEFAUT :
-        action = 0;
+            break;
+            
+        case AX12_POSITION :
+            GetPositionAx12();
             break;
     }
 }
-   
 
-unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
-                                unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
-                                unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3)
+void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
+                              unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
+                              unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
+                              unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
+                              unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5)
 {
-   char TabPompeBras[15];
-   unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
+    char TabBrasCentralPR[25];
+    unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1, GPosition5_1;
+      
+    GPosition1_1=((unsigned long)GPosition1*341/1000);
+    GPosition2_1=((unsigned long)GPosition2*341/1000);
+    GPosition3_1=((unsigned long)GPosition3*341/1000);
+    GPosition4_1=((unsigned long)GPosition4*341/1000);
+    GPosition5_1=((unsigned long)GPosition5*341/1000);
    
-   GPosition1_1=((unsigned long)GPosition1*341/1000);
-   GPosition2_1=((unsigned long)GPosition2*341/1000);
-   GPosition3_1=((unsigned long)GPosition3*341/1000);
-   
-   TabPompeBras[0] = ID1;
-   TabPompeBras[1] = GPosition1_1;
-   TabPompeBras[2] = GPosition1_1>>8; 
-   TabPompeBras[3] = GSpeed1; 
-   TabPompeBras[4] = GSpeed1>>8;               /// (haut, milieu, bas)  
+    TabBrasCentralPR[0] = ID1;
+    TabBrasCentralPR[1] = GPosition1_1;
+    TabBrasCentralPR[2] = GPosition1_1>>8; 
+    TabBrasCentralPR[3] = GSpeed1; 
+    TabBrasCentralPR[4] = GSpeed1>>8;               
              
-   TabPompeBras[5] = ID2;
-    TabPompeBras[6] = GPosition2_1;
-    TabPompeBras[7] = GPosition2_1>>8;
-    TabPompeBras[8] = GSpeed2;
-    TabPompeBras[9] = GSpeed2>>8;
+    TabBrasCentralPR[5] = ID2;
+    TabBrasCentralPR[6] = GPosition2_1;
+    TabBrasCentralPR[7] = GPosition2_1>>8;
+    TabBrasCentralPR[8] = GSpeed2;
+    TabBrasCentralPR[9] = GSpeed2>>8;
    
-    TabPompeBras[10] = ID3;
-    TabPompeBras[11] = GPosition3_1;
-    TabPompeBras[12] = GPosition3_1>>8;
-    TabPompeBras[13] = GSpeed3;
-    TabPompeBras[14] = GSpeed3>>8  ;  
+    TabBrasCentralPR[10] = ID3;
+    TabBrasCentralPR[11] = GPosition3_1;
+    TabBrasCentralPR[12] = GPosition3_1>>8;
+    TabBrasCentralPR[13] = GSpeed3;
+    TabBrasCentralPR[14] = GSpeed3>>8  ;  
+    
+    TabBrasCentralPR[15] = ID4;
+    TabBrasCentralPR[16] = GPosition4_1;
+    TabBrasCentralPR[17] = GPosition4_1>>8;
+    TabBrasCentralPR[18] = GSpeed4;
+    TabBrasCentralPR[19] = GSpeed4>>8  ; 
+    
+    TabBrasCentralPR[20] = ID5;
+    TabBrasCentralPR[21] = GPosition5_1;
+    TabBrasCentralPR[22] = GPosition5_1>>8;
+    TabBrasCentralPR[23] = GSpeed5;
+    TabBrasCentralPR[24] = GSpeed5>>8  ; 
                         
-   BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
-   wait(TIME);
-   
-   
-   while (((unsigned short)(HautBrasPompe->Get_Position()*10)>GPosition3*105/100) || ((unsigned short)(HautBrasPompe->Get_Position()*10)<GPosition3*95/100)) {
-        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
-        wait(TIME*5);
+    BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+    wait(TIME);
+      
+    /*  
+    while (((unsigned short)(BaseBrasCentralPR->Get_Position()*10)>GPosition2*108/100) || ((unsigned short)(BaseBrasCentralPR->Get_Position()*10)<GPosition2*92/100)) {
+        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+        pc.printf("1 \n\r ");
+        wait(5*TIME);
+    }
+        
+     while (((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)>GPosition3*108/100) || ((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)<GPosition3*92/100)) {
+        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+        pc.printf("2 \n\r ");
+        wait(5*TIME);
+    }
+        
+     while (((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)>GPosition5*108/100) || ((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)<GPosition5*92/100)) {
+        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+        pc.printf("3 \n\r ");
+        wait(5*TIME);
+    }    
+    
+     while (((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)>GPosition4*108/100) || ((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)<GPosition4*92/100)) {
+        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+        pc.printf("4 \n\r ");
+        wait(5*TIME);
     }
     
-    while (((unsigned short)(MilieuBrasPompe->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(MilieuBrasPompe->Get_Position()*10)<GPosition2*95/100)) {
-        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
-        wait(TIME*5);
-    }
-    
-    
-    while (((unsigned short)(BasBrasPompe->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(BasBrasPompe->Get_Position()*10)<GPosition1*95/100)) {
-        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
-        wait(TIME*5);
+    while (((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)>GPosition1*108/100) || ((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)<GPosition1*92/100)) {
+        BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+        pc.printf("5 \n\r ");
+        wait(5*TIME);
     }
-    
-   
-    
-}
-
-  
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_position                                               */
-/* DESCRIPTION  : Fonction qui place les bras en position verticale                     */
-/****************************************************************************************/
-void Initialisation_position(unsigned char choix){
-    if (choix == 1){
-        multiple_myAX12->multiple_goal_and_speed(4,TAB1);
-        wait(TIME);  
-    }
-    
-    else if (choix == 2){
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB21);
-        wait(TIME);
-    }
-    else if (choix == 3){
-        BrasPompeAx12->multiple_goal_and_speed(3,TabBrasPompePosInit);
-        wait(TIME);
-    }
-    
+    */
     
 }
 
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise                                                     */
-/* DESCRIPTION  : Fonction qui prepare le robot pour prendre les modules                */
-/****************************************************************************************/
-void Preparation_prise(unsigned char choix){    
-    if (choix == 1){
-        multiple_myAX12->multiple_goal_and_speed(4,TAB2);
-        wait(TIME);
-    }
+
+void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
+                              unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
+{
+    char TabBrasGauchePR[10];
+    unsigned short GPosition1_1, GPosition2_1;
+      
+    GPosition1_1=((unsigned long)GPosition1*341/1000);
+    GPosition2_1=((unsigned long)GPosition2*341/1000);
+   
+    TabBrasGauchePR[0] = ID1;
+    TabBrasGauchePR[1] = GPosition1_1;
+    TabBrasGauchePR[2] = GPosition1_1>>8; 
+    TabBrasGauchePR[3] = GSpeed1; 
+    TabBrasGauchePR[4] = GSpeed1>>8;               
+             
+    TabBrasGauchePR[5] = ID2;
+    TabBrasGauchePR[6] = GPosition2_1;
+    TabBrasGauchePR[7] = GPosition2_1>>8;
+    TabBrasGauchePR[8] = GSpeed2;
+    TabBrasGauchePR[9] = GSpeed2>>8;
     
-    else if (choix == 2){
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB22);
-        wait(TIME);
-    }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_haut                                                         */
-/* DESCRIPTION  : Fonction qui prend et stocke les modules dans la position haute       */
-/****************************************************************************************/
-void Stockage_haut(unsigned char choix){
-    if (choix == 1){
-        multiple_myAX12->multiple_goal_and_speed(4,TAB3);
-        wait(TIME);
-        multiple_myAX12->multiple_goal_and_speed(4,TAB4);
-        wait(TIME);
-        multiple_myAX12->multiple_goal_and_speed(4,TAB5);
-        wait(TIME);
-        multiple_myAX12->multiple_goal_and_speed(4,TAB6);
-        wait(TIME);
-    }
-    
-    else if (choix == 2){
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
-        wait(TIME);
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB24);
-        wait(TIME);
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
-        wait(TIME);
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
-        wait(TIME);
-    }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_bas                                                          */
-/* DESCRIPTION  : Fonction qui prend et stocke un module dans la pince                  */
-/****************************************************************************************/
-void Stockage_bas(unsigned char choix){
-    if (choix == 1){
-        multiple_myAX12->multiple_goal_and_speed(4,TAB3);
-        wait(TIME);
-        multiple_myAX12->multiple_goal_and_speed(4,TAB7);
-        wait(TIME);
-    } 
-    
-    else if (choix == 2){
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
-        wait(TIME);
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB27);
-        wait(TIME);
-    }       
+                        
+    BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
+    wait(TIME);
+      
 }
 
-/****************************************************************************************/
-/* FUNCTION NAME: Deposer                                                               */
-/* DESCRIPTION  : Fonction qui permet de déposer le module                              */
-/****************************************************************************************/
-void Deposer(unsigned char choix){
-    if (choix == 1){
-        multiple_myAX12->multiple_goal_and_speed(4,TAB9);
-        wait(TIME);
-    }  
-    
-    else if (choix == 2){
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB29);
-        wait(TIME);
-    }        
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depot_bas                                                 */
-/* DESCRIPTION  : Fonction qui prépare le depos d'un module en bas                      */
-/****************************************************************************************/
-void Preparation_depot_bas(unsigned char choix){
-    if (choix == 1){
-        multiple_myAX12->multiple_goal_and_speed(4,TAB8);
-        wait(TIME);
-    }
+void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
+                    unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
+{
+    char TabBrasDroitPR[10];
+    unsigned short GPosition1_1, GPosition2_1;
+      
+    GPosition1_1=((unsigned long)GPosition1*341/1000);
+    GPosition2_1=((unsigned long)GPosition2*341/1000);
+   
+    TabBrasDroitPR[0] = ID1;
+    TabBrasDroitPR[1] = GPosition1_1;
+    TabBrasDroitPR[2] = GPosition1_1>>8; 
+    TabBrasDroitPR[3] = GSpeed1; 
+    TabBrasDroitPR[4] = GSpeed1>>8;               
+             
+    TabBrasDroitPR[5] = ID2;
+    TabBrasDroitPR[6] = GPosition2_1;
+    TabBrasDroitPR[7] = GPosition2_1>>8;
+    TabBrasDroitPR[8] = GSpeed2;
+    TabBrasDroitPR[9] = GSpeed2>>8;
     
-    else if (choix == 2){
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
-        wait(TIME);
-    }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depot_haut                                                */
-/* DESCRIPTION  : Fonction qui prépare le depos d'un module en haut                     */
-/****************************************************************************************/
-void Preparation_depot_haut(unsigned char choix){
-    if (choix == 1){
-        multiple_myAX12->multiple_goal_and_speed(4,TAB6);
-        wait(TIME);
-        multiple_myAX12->multiple_goal_and_speed(4,TAB5);
-        wait(TIME);
-        multiple_myAX12->multiple_goal_and_speed(4,TAB10);
-        wait(TIME); 
-        multiple_myAX12->multiple_goal_and_speed(4,TAB8);
-        wait(TIME);   
-    }
-    
-    else if (choix == 2){
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
-        wait(TIME);
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
-        wait(TIME);
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB30);
-        wait(TIME); 
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
-        wait(TIME);   
-    }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Pousser_module                                                        */
-/* DESCRIPTION  : Fonction qui permet pousser le module situé à l'entrée de la bas      */
-/****************************************************************************************/
-void Pousser_module(unsigned char choix){
-    if (choix == 1){
-        multiple_myAX12->multiple_goal_and_speed(4,TAB11);
-        wait(TIME);   
-    }
-    
-    else if (choix == 2){
-        multiple2_myAX12->multiple_goal_and_speed(4,TAB31);
-        wait(TIME);   
-    }
+                        
+    BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
+    wait(TIME);
+      
 }
 
 /****************************************************************************************/
@@ -485,63 +520,7 @@
     
     msgTx.id = SERVO_AX12_END;
     msgTx.len = 1;  
-    msgTx.data[0] = AX12_PREPARATION_PRISE;
+    msgTx.data[0] = 0;
     can.write(msgTx);  
 }
-
-
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: Check_positionAX12                                                    */
-/* DESCRIPTION  : Fonction qui permet de verifier la position des AX12                  */
-/****************************************************************************************/
-void Check_positionAX12(char* TAB, unsigned char choix){
-    int k=1, i=0;
-    static float TAB_POS_TH[4];
-    
-    CANMessage msgTx=CANMessage();
-    msgTx.id=SERVO_AX12_POSITION;
-    msgTx.len=5;
-    
-    //PERMET DE VERIFIER LA POSITION D'UN AX12
-    TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3);
-    TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3);
-    TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3);
-    TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3);
-    TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3);
-    TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3);
-    TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3);
-    TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3);
-     
-    for(i=0; i<4; i++){
-        TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8);    
-        k=k+5;
-    }
-    
-    for(i=0; i<4; i++){
-        if (choix == 1){
-            if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
-                TAB_POSITION[i] = 1;
-                }
-            else if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
-                TAB_POSITION[i] = 0;
-                }
-            }
-        else if (choix == 2){
-            if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
-                TAB_POSITION[i] = 1;
-                }
-            else if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
-                TAB_POSITION[i] = 0;
-                }
-            }
-     }
-    
-    msgTx.data[0] = choix;
-    for(i=1; i<5; i++){
-        msgTx.data[i] = TAB_POSITION[i];
-    }
-    can.write(msgTx);
-}            
-            
\ No newline at end of file
+ 
\ No newline at end of file