Revision 5:7e1c328c5d50, committed 2017-05-25

Comitter:

matthieuvignon

Date:

Thu May 25 06:35:22 2017 +0000

Parent:

3:43843ab8af47

Commit message:

Version Gatien Nuit

Changed in this revision

--- a/Actionneur.cpp	Sun May 21 16:10:38 2017 +0000
+++ b/Actionneur.cpp	Thu May 25 06:35:22 2017 +0000
@@ -12,11 +12,16 @@
 extern void GetPositionAx12(void);
 
 extern PwmOut Pompe;
+#define MULT_TIMEOUT 30
+
+unsigned char timeout=0;
+Timeout flipper;
 
 
 AX12 *Ax12_HautBrasModuleAvant, *Ax12_DevantPinceDroite, *Ax12_DevantPinceGauche, *Ax12_MilieuBrasModuleAvant, *BrasModuleAx12, *TabBrasModule; 
 AX12 *Ax12_MilieuBrasPompe, *Ax12_HautBrasPompe, *Ax12_BasBrasPompe, *BrasPompeAx12, *TabBrasPompe;                                 
-AX12 *Ax12_HautBrasAspirateur, *Ax12_BasBrasAspirateur, *BrasAspirateurAx12, *TabBrasAspirateur, *Ax12_FunnyAction;                                 
+AX12 *Ax12_HautBrasAspirateur, *Ax12_BasBrasAspirateur, *BrasAspirateurAx12, *TabBrasAspirateur, *Ax12_FunnyAction;
+AX12 *Ax12_Porte;                                 
                      
                    
                             /*   ANGLE   */
@@ -50,7 +55,7 @@
 
 void initialisation_AX12(void) 
 {
-    short vitesse=700;
+    short vitesse=900;
     
     
     Ax12_HautBrasPompe = new AX12(p9, p10, 3, 1000000);  
@@ -62,16 +67,21 @@
     Ax12_DevantPinceGauche = new AX12(p13, p14, 6, 1000000);
     Ax12_DevantPinceDroite = new AX12(p13, p14, 7, 1000000);
     
-    Ax12_HautBrasAspirateur = new AX12(p28, p27, 9, 1000000);  
-    Ax12_BasBrasAspirateur = new AX12(p28, p27, 10, 1000000);
-    Ax12_FunnyAction = new AX12(p28, p27, 15, 1000000);
-    
-    //AX12_Bras = new AX12(p9, p10, 1, 1000000);
+    #ifdef AVANT
+        Ax12_HautBrasAspirateur = new AX12(p28, p27, 9, 1000000);  
+        Ax12_BasBrasAspirateur = new AX12(p28, p27, 10, 1000000);
+        Ax12_FunnyAction = new AX12(p28, p27, 15, 1000000);
+        BrasAspirateurAx12 = new AX12(p28, p27,0xFE,1000000);
+    #endif
     
     BrasPompeAx12 = new AX12(p9,p10,0xFE,1000000);
-    BrasModuleAx12 = new AX12(p13, p14, 0xFE, 1000000);
-    BrasAspirateurAx12 = new AX12(p28, p27,0xFE,1000000);
+    BrasModuleAx12 = new AX12(p13, p14, 0xFE, 1000000);   
+    
     
+    #ifdef ARRIERE
+        Ax12_Porte = new AX12(p28, p27, 8, 1000000);  
+    #endif
+        
     Ax12_HautBrasPompe->Set_Goal_speed(vitesse);  
     Ax12_MilieuBrasPompe->Set_Goal_speed(vitesse); 
     Ax12_BasBrasPompe->Set_Goal_speed(vitesse); 
@@ -79,9 +89,14 @@
     Ax12_MilieuBrasModuleAvant->Set_Goal_speed(vitesse); 
     Ax12_DevantPinceGauche->Set_Goal_speed(vitesse); 
     Ax12_DevantPinceDroite->Set_Goal_speed(vitesse);
-    Ax12_HautBrasAspirateur->Set_Goal_speed(vitesse);
-    Ax12_BasBrasAspirateur->Set_Goal_speed(vitesse); 
-    Ax12_FunnyAction->Set_Goal_speed(vitesse); 
+     #ifdef AVANT
+        Ax12_HautBrasAspirateur->Set_Goal_speed(vitesse);
+        Ax12_BasBrasAspirateur->Set_Goal_speed(vitesse); 
+        Ax12_FunnyAction->Set_Goal_speed(vitesse); 
+    #endif
+    #ifdef ARRIERE
+        Ax12_Porte->Set_Goal_speed(vitesse); 
+    #endif
     
     Ax12_HautBrasPompe->Set_Mode(0); 
     Ax12_MilieuBrasPompe->Set_Mode(0); 
@@ -90,9 +105,14 @@
     Ax12_MilieuBrasModuleAvant->Set_Mode(0);
     Ax12_DevantPinceGauche->Set_Mode(0);
     Ax12_DevantPinceDroite->Set_Mode(0);
-    Ax12_HautBrasAspirateur->Set_Mode(0);
-    Ax12_BasBrasAspirateur->Set_Mode(0);
-    Ax12_FunnyAction->Set_Mode(0);
+    #ifdef AVANT
+        Ax12_HautBrasAspirateur->Set_Mode(0);
+        Ax12_BasBrasAspirateur->Set_Mode(0);
+        Ax12_FunnyAction->Set_Mode(0);
+    #endif
+    #ifdef ARRIERE
+        Ax12_Porte->Set_Mode(0);
+    #endif
 } 
 
 void GetPositionAx12(void) {
@@ -105,16 +125,24 @@
     pc.printf("Gauche Av: %lf \n\r ",   Ax12_DevantPinceGauche->Get_Position() );  
     pc.printf("Droite Av: %lf \n\r ",   Ax12_DevantPinceDroite->Get_Position() );  
     
-    pc.printf("\n\r BRAS ASPIRATEUR \n\r");
-    pc.printf("Haut Asp : %lf \n\r ",   Ax12_HautBrasAspirateur->Get_Position()      );
-    pc.printf("Bas Asp : %lf \n\r ",   Ax12_BasBrasAspirateur->Get_Position()    );
-    pc.printf("Funny Action : %lf \n\r ",   Ax12_FunnyAction->Get_Position()    );
+    #ifdef AVANT
+        pc.printf("\n\r BRAS ASPIRATEUR \n\r");
+        pc.printf("Haut Asp : %lf \n\r ",   Ax12_HautBrasAspirateur->Get_Position()      );
+        pc.printf("Bas Asp : %lf \n\r ",   Ax12_BasBrasAspirateur->Get_Position()    );
+        pc.printf("Funny Action : %lf \n\r ",   Ax12_FunnyAction->Get_Position()    );
+    #endif
+    
     
     pc.printf("\n\r BRAS POMPE \n\r");
     pc.printf("Haut Pompe : %lf \n\r ",    Ax12_HautBrasPompe->Get_Position()      );
     pc.printf("Milieu Pompe : %lf \n\r ",   Ax12_MilieuBrasPompe->Get_Position()      );
     pc.printf("Bas Pompe : %lf \n\r ",   Ax12_BasBrasPompe->Get_Position()      );
     
+    #ifdef ARRIERE
+        pc.printf("\n\r PORTE \n\r");
+        pc.printf("Haut Pompe : %lf \n\r ",   Ax12_Porte->Get_Position()      );
+    #endif
+    
 }
 
 
@@ -130,7 +158,17 @@
     
     
     
-        switch(etat_ax12){         
+        switch(etat_ax12){                    
+            
+             case AX12_OUVRIR_PORTE:
+                Ax12_Porte->Set_Secure_Goal(60);
+                Fin_action();
+                break;
+            
+             case AX12_FERMER_PORTE:
+                Ax12_Porte->Set_Secure_Goal(240);
+                Fin_action();
+                break;
             
             case AX12_GET_POSITION:
                 GetPositionAx12();
@@ -138,7 +176,7 @@
                 break;
             
             case AX12_POS_INIT_POMPE_JAUNE_AVANT:
-                speed=750;
+                speed=1023;
                 
                 GoalPos3=1443;
                 GoalPos2=625;
@@ -148,7 +186,7 @@
                 break;
             
             case AX12_VIDER_FUSEE_POMPE_JAUNE_AVANT:
-                speed=750;
+                speed=1023;
                 
                 GoalPos3=1440;
                 GoalPos2=603;
@@ -203,7 +241,7 @@
                 
              case AX12_REMISE_PLACE_BRAS_POMPE_AVANT:
              
-                speed=750;
+                speed=1023;
                 
                 GoalPos3=2200;
                 GoalPos2=214;
@@ -225,9 +263,19 @@
                 Fin_action();
                 break; 
                 
+            case AX12_REPOS_BRAS_POMPE_AVANT:
+                speed=1023;
+                
+                GoalPos3=1890;
+                GoalPos2=242;
+                GoalPos1=1500;
+                mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
+                Fin_action();
+                break; 
+                
                 
             case AX12_POS_INIT_POMPE_JAUNE_ARRIERE:
-                speed=750;
+                speed=1023;
                 
                 GoalPos3=1615;
                 GoalPos2=2349;
@@ -239,7 +287,7 @@
             
             
             case AX12_VIDER_FUSEE_POMPE_JAUNE_ARRIERE:
-                speed=750;
+                speed=1023;
                 
                 GoalPos3=1650;
                 GoalPos2=2380;
@@ -261,14 +309,14 @@
                 break;
                 
              case AX12_POSE_MODULE_POMPE_JAUNE_ARRIERE:
-                speed=750;
+                speed=300;
                 
                 GoalPos3=401;
                 GoalPos2=2340;
                 GoalPos1=1094;
                 mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
                 GoalPos3=401;
-                GoalPos2=2800;
+                GoalPos2=2250;      
                 GoalPos1=80;
                 mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
                   
@@ -279,7 +327,7 @@
                 
             
             case AX12_POSE_MODULE_POMPE_BLEU_ARRIERE:
-                speed=750;        
+                speed=300;        
             
                 GoalPos3=384;
                 GoalPos2=2774;
@@ -297,7 +345,7 @@
                 
              case AX12_REMISE_PLACE_BRAS_POMPE_ARRIERE:
              
-                speed=750;
+                speed=1023;
             
                 GoalPos3=450;   //449
                 GoalPos2=2800;  //2833
@@ -311,8 +359,8 @@
                 GoalPos2=2750;
                 GoalPos1=600;
                 mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
-                GoalPos3=1557;
-                GoalPos2=2500;
+                GoalPos3=1480;
+                GoalPos2=2550;
                 GoalPos1=600;
                 mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
                  
@@ -322,12 +370,22 @@
                 mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
                 
                 Fin_action();
-                break;        
+                break;   
+                
+            case AX12_REPOS_BRAS_POMPE_ARRIERE:
+                speed=1023;
+                
+                GoalPos3=1209;
+                GoalPos2=2693;
+                GoalPos1=1500;
+                mvtBrasAvantPompe(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
+                Fin_action();
+                break;
+                 
           
             
             case AX12_INITIALISATION :
-                speed = 750;
-                
+                speed = 1023;
                 GoalPos4=2200;
                 GoalPos5=1522;
                 GoalPos6=2528;
@@ -342,7 +400,7 @@
                 break;
                             
             case AX12_PREPARATION_PRISE :
-                speed = 750;
+                speed = 1023;
                 
                 GoalPos4=970;
                 GoalPos5=2060;
@@ -353,6 +411,7 @@
                 break;
                             
             case AX12_STOCKAGE_HAUT :
+                speed = 1023;
                 GoalPos4=970;
                 GoalPos5=2060;
                 GoalPos6=2520;
@@ -382,6 +441,7 @@
                 break;
                             
             case AX12_STOCKAGE_BAS :
+                speed = 1023;
                 GoalPos4=970;
                 GoalPos5=2060;
                 GoalPos6=2460;
@@ -407,6 +467,7 @@
                 break;
                             
             case AX12_DEPOSER :
+                speed = 1023;
                 GoalPos4=1580;
                 GoalPos5=660;
                 GoalPos6=2200;
@@ -416,6 +477,7 @@
                 break;
                                             
             case AX12_PREPARATION_DEPOT_HAUT :
+                speed = 1023;
                 GoalPos4=1970;
                 GoalPos5=2140;
                 GoalPos6=2000;
@@ -442,6 +504,7 @@
                 break;
                             
             case AX12_POUSSER_MODULE :
+                speed = 1023;
                 GoalPos4=1880;
                 GoalPos5=440;
                 GoalPos6=2460;
@@ -474,7 +537,7 @@
                 GoalPos10=2370;
                 GoalPos15=1250;
                 mvtBrasAspirateur(9, speed, GoalPos9, 10, speed, GoalPos10, 15, speed, GoalPos15);
-                wait(10*TIME);
+                wait(1);
                 speed = 1023;
                 GoalPos9=540;
                 GoalPos10=2370;
@@ -497,6 +560,7 @@
 {
    char TabPompeBras[15];
    unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
+   Timer timeout;
    
    GPosition1_1=((unsigned long)GPosition1*341/1000);
    GPosition2_1=((unsigned long)GPosition2*341/1000);
@@ -523,27 +587,35 @@
    BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
    wait(TIME);
    
-   
-   while (((unsigned short)(Ax12_HautBrasPompe->Get_Position()*10)>GPosition3*105/100) || ((unsigned short)(Ax12_HautBrasPompe->Get_Position()*10)<GPosition3*95/100)) {
-        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
-        wait(TIME*5);
+   timeout.start();
+   while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_HautBrasPompe->Get_Position()*10)>GPosition3*110/100) 
+                                    || ((unsigned short)(Ax12_HautBrasPompe->Get_Position()*10)<GPosition3*90/100))) {
+        Ax12_HautBrasPompe->Set_Secure_Goal(GPosition3/10) ;
+        wait(TIME);   
     }
     
-    while (((unsigned short)(Ax12_MilieuBrasPompe->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(Ax12_MilieuBrasPompe->Get_Position()*10)<GPosition2*95/100)) {
-        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
-        wait(TIME*5);
+     
+    //timeout.reset();
+    while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_MilieuBrasPompe->Get_Position()*10)>GPosition2*110/100) 
+                                    || ((unsigned short)(Ax12_MilieuBrasPompe->Get_Position()*10)<GPosition2*90/100))) {
+        Ax12_MilieuBrasPompe->Set_Secure_Goal(GPosition2/10) ;
+        wait(TIME);
     }
-    
-    
-    while (((unsigned short)(Ax12_BasBrasPompe->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(Ax12_BasBrasPompe->Get_Position()*10)<GPosition1*95/100)) {
-        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
-        wait(TIME*5);
+   
+   //timeout.reset();
+   while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_BasBrasPompe->Get_Position()*10)>GPosition1*110/100) 
+                                    || ((unsigned short)(Ax12_BasBrasPompe->Get_Position()*10)<GPosition1*90/100))) {
+        Ax12_BasBrasPompe->Set_Secure_Goal(GPosition1/10) ;
+        wait(TIME);
     }
+   timeout.reset();
     
    
     
 }
 
+
+
 unsigned char mvtBrasAvantModule(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,
@@ -552,6 +624,7 @@
     
     char TabBrasAvant[20];
     unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1;
+   Timer timeout;
    
    GPosition1_1=((unsigned long)GPosition1*341/1000);
    GPosition2_1=((unsigned long)GPosition2*341/1000);
@@ -584,24 +657,35 @@
                         
    BrasModuleAx12->multiple_goal_and_speed(4,TabBrasAvant) ;
    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);
+   timeout.start();
+   while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_HautBrasModuleAvant->Get_Position()*10)>GPosition1*105/100) 
+                            || ((unsigned short)(Ax12_HautBrasModuleAvant->Get_Position()*10)<GPosition1*95/100))) {
+        Ax12_HautBrasModuleAvant->Set_Secure_Goal(GPosition1/10) ;
+        wait(TIME);
     }
+    timeout.reset();
     
-    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 ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_MilieuBrasModuleAvant->Get_Position()*10)>GPosition2*105/100) 
+                            || ((unsigned short)(Ax12_MilieuBrasModuleAvant->Get_Position()*10)<GPosition2*95/100))) {
+        Ax12_MilieuBrasModuleAvant->Set_Secure_Goal(GPosition2/10) ;
+        wait(TIME);
     }
-    
+    timeout.reset();
     
-    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 ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_DevantPinceGauche->Get_Position()*10)>GPosition3*105/100) 
+                            || ((unsigned short)(Ax12_DevantPinceGauche->Get_Position()*10)<GPosition3*95/100))) {
+        Ax12_DevantPinceGauche->Set_Secure_Goal(GPosition3/10) ;
+        wait(TIME);
     }
-    */
+    timeout.reset();
+  
+    while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_DevantPinceDroite->Get_Position()*10)>GPosition4*105/100) 
+                            || ((unsigned short)(Ax12_DevantPinceDroite->Get_Position()*10)<GPosition4*95/100))) {
+        Ax12_DevantPinceDroite->Set_Secure_Goal(GPosition4/10) ;
+        wait(TIME);
+    }
    
     
 }
@@ -612,6 +696,8 @@
 {
     char TabAspirateurBras[15];
    unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
+   Timer timeout;
+   
    
    GPosition1_1=((unsigned long)GPosition1*341/1000);
    GPosition2_1=((unsigned long)GPosition2*341/1000);
@@ -638,18 +724,26 @@
    BrasAspirateurAx12->multiple_goal_and_speed(3,TabAspirateurBras) ;
    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);
+   timeout.start();
+   while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_HautBrasAspirateur->Get_Position()*10)>GPosition1*105/100) 
+                                            || ((unsigned short)(Ax12_HautBrasAspirateur->Get_Position()*10)<GPosition1*95/100))) {
+        Ax12_HautBrasAspirateur->Set_Secure_Goal(GPosition1/10) ;
+        wait(TIME);
     }
+    timeout.reset();
+
+    while ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_BasBrasAspirateur->Get_Position()*10)>GPosition2*105/100) 
+                                            || ((unsigned short)(Ax12_BasBrasAspirateur->Get_Position()*10)<GPosition2*95/100))) {
+        Ax12_BasBrasAspirateur->Set_Secure_Goal(GPosition2/10) ;
+        wait(TIME);
+    }
+    timeout.reset();
     
-    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 ((timeout.read_ms()<MULT_TIMEOUT) || (((unsigned short)(Ax12_FunnyAction->Get_Position()*10)>GPosition3*105/100) 
+                                            || ((unsigned short)(Ax12_FunnyAction->Get_Position()*10)<GPosition3*95/100))) {
+        Ax12_FunnyAction->Set_Secure_Goal(GPosition3/10) ;
+        wait(TIME);
+    }
     
 }
 
@@ -665,7 +759,7 @@
     
     msgTx.id = SERVO_AX12_END;
     msgTx.len = 1;  
-    msgTx.data[0] = AX12_PREPARATION_PRISE;
+    msgTx.data[0] = 0x96;
     can.write(msgTx);  
 }
 

--- a/Actionneur.h	Sun May 21 16:10:38 2017 +0000
+++ b/Actionneur.h	Thu May 25 06:35:22 2017 +0000
@@ -10,7 +10,7 @@
 #define AX12_PREPARATION_DEPOT_HAUT 6
 #define AX12_POUSSER_MODULE 7
 
-#define AX12_POS_INIT_POMPE_JAUNE_AVANT 10
+#define AX12_POS_INIT_POMPE_JAUNE_AVANT 10      //0A
 #define AX12_VIDER_FUSEE_POMPE_JAUNE_AVANT 11
 #define AX12_POSE_MODULE_POMPE_JAUNE_AVANT 12
 #define AX12_POSE_MODULE_POMPE_BLEU_AVANT 14
@@ -18,17 +18,20 @@
 #define AX12_REPOS_BRAS_POMPE_AVANT 16
 
 
-#define AX12_POS_INIT_POMPE_JAUNE_ARRIERE 30
+#define AX12_POS_INIT_POMPE_JAUNE_ARRIERE 30        //1E
 #define AX12_VIDER_FUSEE_POMPE_JAUNE_ARRIERE 31
 #define AX12_POSE_MODULE_POMPE_JAUNE_ARRIERE  32
 #define AX12_POSE_MODULE_POMPE_BLEU_ARRIERE  34
-#define AX12_REMISE_PLACE_BRAS_POMPE_ARRIERE  35
+#define AX12_REMISE_PLACE_BRAS_POMPE_ARRIERE  35           //23
 #define AX12_REPOS_BRAS_POMPE_ARRIERE  36
 
 #define AX12_DESCENDRE_ASPIRATEUR 20
 #define AX12_MONTER_ASPIRATEUR 21
 #define AX12_FUNNY_ACTION 22
 
+#define AX12_OUVRIR_PORTE 40
+#define AX12_FERMER_PORTE 41
+
 #define AX12_GET_POSITION 100
 #define AX12_DEFAUT 0
 

--- a/CAN.cpp	Sun May 21 16:10:38 2017 +0000
+++ b/CAN.cpp	Thu May 25 06:35:22 2017 +0000
@@ -52,6 +52,45 @@
     if(FIFO_occupation!=0) {
         
         switch(msgRxBuffer[FIFO_lecture].id) {
+            #ifdef ARRIERE
+            case ACKNOWLEDGE_GLOBAL_JACK:
+                jack.fall(NULL);
+                EnvoieJack=0;
+                TimeJack.stop();
+                break; 
+            #endif
+            #ifdef AVANT
+            case CHECK_CARTE_AVANT:
+                EtatCarteAvant = 1;
+                Cote = 1; //Avant
+                //ACK de reception des actions a effectuer
+                msgTx.id = 0x73;
+                msgTx.len = 0;
+                can.write(msgTx);
+                wait_us(50);
+                break;
+            #endif
+                
+            #ifdef ARRIERE    
+            case CHECK_CARTE_ARRIERE:
+                EtatCarteArriere = 1;
+                Cote = 2; //Arriere
+                //ACK de reception des actions a effectuer
+                msgTx.id = 0x74;
+                msgTx.len = 0;
+                can.write(msgTx);
+                wait_us(50);
+                break;
+            #endif
+            
+            case GLOBAL_GAME_END:
+                EtatGameEnd = 1;  
+                break;
+            
+            case GLOBAL_FUNNY_ACTION:
+                EtatFunnyAction  = msgRxBuffer[FIFO_lecture].data[0];
+                Cote = msgRxBuffer[FIFO_lecture].data[1];
+                break;
             
             case ELECTROVANNE:
                 EtatElectroVanne  = msgRxBuffer[FIFO_lecture].data[0];
@@ -62,11 +101,22 @@
                 ActionPompe = 1;
                 EtatPompe  = msgRxBuffer[FIFO_lecture].data[0];
                 Cote = msgRxBuffer[FIFO_lecture].data[1];
+                
+                msgTx.id = ACKNOWLEDGE_POMPES;
+                msgTx.len = 1;
+                msgTx.data[0] = POMPE_PWM;
+                can.write(msgTx);
+                wait_us(50);
                 break;
                 
             case TURBINE:
                 EtatTurbine = msgRxBuffer[FIFO_lecture].data[0];
                 Cote = msgRxBuffer[FIFO_lecture].data[1];
+                
+                msgTx.id = SERVO_AX12_ACK;
+                msgTx.len = 1;
+                msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+                can.write(msgTx);
                 break;
                 
             case LANCEUR:
@@ -97,6 +147,7 @@
                 GetPositionAx12();
                 break;
             
+            
         }
         action_a_effectuer=1;
         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;

--- a/all_includes.h	Sun May 21 16:10:38 2017 +0000
+++ b/all_includes.h	Thu May 25 06:35:22 2017 +0000
@@ -5,24 +5,27 @@
 #include "cmsis.h"
 
 #define SIZE_FIFO 20 
-#define TIME 0.5
+#define TIME 0.02
 
 #define AVT 1
 #define ARR 2
 
 
 /********A MODIFIER*******/
+//#define ARRIERE 0
 #define AVANT 0
-
+//#define CARTE ARR
 #define CARTE AVT
 /*************************/
 
-extern unsigned char EtatPompe, EtatLanceur, EtatAx12, Cote, ActionAx12, EtatTurbine, EtatElectroVanne;
-extern unsigned char action_a_effectuer, ActionPompe;
+extern unsigned char EtatPompe, EtatLanceur, EtatAx12, Cote, ActionAx12, EtatTurbine, EtatElectroVanne, EtatFunnyAction, EtatGameEnd, EnvoieJack;
+extern unsigned char action_a_effectuer, ActionPompe, EtatCarteAvant, EtatCarteArriere;
 
+extern InterruptIn jack;
 extern PwmOut Pompe;
-
+extern Timeout flipper;
 extern unsigned char Cote;
+extern Timer TimeJack;
                               
 void GetPositionAx12(void);
 void SendRawId (unsigned short id);

--- a/ident_crac.h	Sun May 21 16:10:38 2017 +0000
+++ b/ident_crac.h	Thu May 25 06:35:22 2017 +0000
@@ -7,6 +7,8 @@
 #define GLOBAL_START 0x002  // Start
 #define GLOBAL_END_INIT_POSITION 0x005  // Fin positionnement robot avant depart
 #define GLOBAL_FUNNY_ACTION 0x007  // Funny action start  (0: start, 1: stop)
+#define GLOBAL_JACK 0x008 // ID pour lancement match par enlevelement de jack
+#define ACKNOWLEDGE_GLOBAL_JACK 0x009
 
 #define BALISE_STOP 0x003  // Trame stop
 
@@ -55,8 +57,8 @@
 #define CHECK_BALISE 0x060  // Check balise
 #define CHECK_MOTEUR 0x061  // Check moteur
 #define CHECK_IHM 0x062  // Check écran tactile
-#define CHECK_ACTIONNEURS 0x063  // Check actionneurs
-#define CHECK_POMPES 0x064  // Check pompes
+#define CHECK_CARTE_AVANT 0x63  // Check actionneurs
+#define CHECK_CARTE_ARRIERE 0x64  // Check pompes
 #define CHECK_AX12 0x065  // Check AX12
 #define CHECK_OK_TELEMETRE 0x066 // Check telemetre
 

--- a/main.cpp	Sun May 21 16:10:38 2017 +0000
+++ b/main.cpp	Thu May 25 06:35:22 2017 +0000
@@ -1,7 +1,7 @@
 #include "all_includes.h"
 
 Timer t;
-Ticker flipper;
+Timer TimeJack;
 
 CAN can(p30,p29); // Rx&Tx pour le CAN
 CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN
@@ -18,25 +18,28 @@
     PwmOut Pompe(p21);
     PwmOut ElectroVanne(p22);
     PwmOut turbine(p23);
+    
 #endif
 
 #ifdef ARRIERE
     PwmOut Pompe(p21);
     PwmOut MotLanceur(p22);
+    InterruptIn jack(p23);
+    //DigitalIn Jack(p25);
 #endif 
 
 
 
 AnalogIn telemetre(p15);
 
-DigitalIn Jack(p25);
+
 
 
 DigitalOut led(LED1);
 DigitalOut led2(LED2);
 
-unsigned char EtatPompe=0, EtatLanceur=0, EtatAx12=0, ChoixBras=0, ActionAx12=0, EtatTurbine=0, EtatElectroVanne=0;
-unsigned char action_a_effectuer=0, ActionPompe=0;
+unsigned char EtatPompe=0, EtatLanceur=0, EtatAx12=0, ChoixBras=0, ActionAx12=0, EtatTurbine=0, EtatElectroVanne=0, EtatFunnyAction=0, EtatGameEnd=0, EnvoieJack=0;
+unsigned char action_a_effectuer=0, ActionPompe=0, EtatCarteAvant=0, EtatCarteArriere=0;
 
 
 /*
@@ -56,6 +59,19 @@
 PwmOut IRL_2(p22);
 */
 
+/****************************************************************************************/
+/* FUNCTION NAME: jack_ISR                                                              */
+/* DESCRIPTION  : Interruption en changement d'état sur le Jack                         */
+/****************************************************************************************/
+#ifdef ARRIERE
+void jack_ISR (void)
+{
+    led2=1;
+    SendRawId(GLOBAL_JACK);
+    EnvoieJack = 1;
+    TimeJack.start();
+}
+#endif
 
 
     
@@ -72,38 +88,98 @@
     CAN2_wrFilter(TURBINE);
     CAN2_wrFilter(ELECTROVANNE);
     CAN2_wrFilter(0x123);
+     
+    CAN2_wrFilter(CHECK_CARTE_AVANT);
+    CAN2_wrFilter(CHECK_CARTE_ARRIERE); 
+    CAN2_wrFilter(ACKNOWLEDGE_GLOBAL_JACK);
        
     CAN2_wrFilter(SERVO_AX12_ACTION);
     CAN2_wrFilter(SERVO_AX12_ACK);
     CAN2_wrFilter(SERVO_AX12_END);
     CAN2_wrFilter(CHECK_AX12);
+    CAN2_wrFilter(GLOBAL_FUNNY_ACTION);
+    CAN2_wrFilter(GLOBAL_GAME_END);
     
-    initialisation_AX12();
+    
+    
     
     #ifdef AVANT 
-        Pompe.period(0.001);
-        ElectroVanne.period(0.001);
+        Pompe.period(0.00005);
+        ElectroVanne.period(0.00005);
+        turbine.period(0.00005);
     #endif
 
     
     #ifdef ARRIERE
-        Pompe.period(0.001); 
-        MotLanceur.period(0.001);
+        Pompe.period(0.00005); 
+        MotLanceur.period(0.00005);
     #endif
+   
     
+ 
     
     
     while(1) {
         led = !led;
         canProcessRx();//Traitement des trames CAN en attente 
         
+        if (EtatGameEnd==1) {
+            #ifdef AVANT 
+                Pompe.write(0);
+                ElectroVanne.write(0);
+                gerer_turbine(0);
+            #endif
+        
+            #ifdef ARRIERE
+                Pompe.write(0); 
+                MotLanceur.write(0);
+            #endif  
+         
+        }
+        
+        if ((EnvoieJack==1)  && (TimeJack.read_ms()>100)){
+            SendRawId(GLOBAL_JACK); 
+            TimeJack.reset();    
+        }
+        
+           
+             
+        
         
         if (action_a_effectuer==1) {
             action_a_effectuer=0;
             
             if (Cote==CARTE) {
+                #ifdef AVANT
+                    if (EtatCarteAvant==1) {
+                        EtatCarteAvant=0;
+                        initialisation_AX12();
+                        
+                        AX12_automate(1);
+                        AX12_automate(16);
+                        AX12_automate(21);    
+                    }
+                #endif
+                
+                #ifdef ARRIERE
+                    if (EtatCarteArriere==1) {
+                        EtatCarteArriere=0;
+                        
+                        initialisation_AX12();
+                        AX12_automate(1);
+                        AX12_automate(36);
+                        AX12_automate(41);
+                        jack.mode(PullDown); // désactivation de la résistance interne du jack
+                        jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack   
+                    }
+                #endif
+                
+                if (EtatFunnyAction==1){
+                    AX12_automate(22); //Funny Action
+                }
+                
+                
                 if (ActionAx12==1){
-                    led2=!led2;
                     AX12_automate(EtatAx12);
                     ActionAx12=0;
                 }
@@ -115,9 +191,9 @@
                 
                 #ifdef AVANT
                     if (EtatTurbine==1)
-                        gerer_turbine(1);
+                        turbine.write(1);
                     else if (EtatTurbine==0)
-                        gerer_turbine(0);
+                         turbine.write(0);
                                    
                     if (EtatElectroVanne==1)
                         ElectroVanne.write(1);
@@ -126,8 +202,38 @@
                 #endif
                 
                 #ifdef ARRIERE   
-                    if (EtatLanceur==1)
+                    if (EtatLanceur==1) {
                         MotLanceur.write(1);
+                        wait(0.2);
+                        
+                        unsigned char i;
+                        for(unsigned char j=1;j<10;j++) {
+                            for(i=10;i>5;i--) {
+                                float value=(float)i/10.;
+                                MotLanceur.write(value);
+                                wait(0.2);
+                            }
+                        }
+                        MotLanceur.write(0);
+                        
+                        /*   MotLanceur.write(1);
+                        wait(0.2);
+                        MotLanceur.write(0.27);
+                        wait(0.1);
+                        MotLanceur.write(1);
+                        wait(0.1);
+                        MotLanceur.write(0.27);
+                        wait(0.1);
+                        MotLanceur.write(1);
+                        wait(0.1);
+                        MotLanceur.write(0.27);
+                        wait(0.1);
+                        MotLanceur.write(1);
+                        wait(0.1);
+                        MotLanceur.write(0.27);
+                        wait(3);
+                        MotLanceur.write(0);*/
+                    }
                     else if (EtatLanceur==0)
                         MotLanceur.write(0); 
                 #endif
@@ -137,3 +243,8 @@
     }
 }
 
+
+
+
+
+