Diff: main.cpp

Revision:

2:99b1cb0d9f5e

Parent:

1:b3ff77670606

Child:

3:6cbd7d2c78f4

diff -r b3ff77670606 -r 99b1cb0d9f5e main.cpp
--- a/main.cpp	Tue Mar 21 14:46:54 2017 +0000
+++ b/main.cpp	Tue Apr 18 16:02:40 2017 +0000
@@ -1,42 +1,139 @@
 #include "mbed.h"
 #include "AX12.h"
 #include "cmsis.h"
+#include "ident_crac.h"
 
+#define SIZE_FIFO 14
+#define TIME 0.8
+        
                              /*  DECLARATION VARIABLES */
+CAN can1(p30,p29);
+CANMessage msgRxBuffer[SIZE_FIFO];
+unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
+
+extern "C" void mbed_reset();//Pour pouvoir reset la carte
+
+unsigned char action = 0, choix = 0;
+short vitesse=700;
+float angle=0.0;
+float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;  
+AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *six_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; 
+                                 
+                                 
+                    /*       PROTOTYPES DE FONCTIONS ET POINTEURS       */
+                    
+                   
+                    
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx                                                          */
+/* DESCRIPTION  : Fonction de traitement des messages CAN                               */
+/****************************************************************************************/
+void canProcessRx(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: canRx_ISR                                                             */
+/* DESCRIPTION  : Interruption en réception sur le CAN                                  */
+/****************************************************************************************/
+void canRx_ISR (void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendRawId                                                             */
+/* DESCRIPTION  : Fonction qui permet d'envoi une trame vide à un ID                    */
+/****************************************************************************************/
+void SendRawId (unsigned short id);
+
+/****************************************************************************************/
+/* FUNCTION NAME: prendre_modules                                                       */
+/* DESCRIPTION  : Fonction pour prendre des modules lunaires avec la pince              */
+/****************************************************************************************/  
+void Prendre_modules(unsigned char choix);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Poser_modules                                                         */
+/* DESCRIPTION  : Fonction qui permet de déposer des modules lunaires                   */
+/****************************************************************************************/
+void Poser_modules(unsigned char choix);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Funny_action                                                          */
+/* DESCRIPTION  : Fonction qui permet d'effectuer la Funny Action en fin de match       */
+/****************************************************************************************/
+void Funny_Action(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_modules                                                       */
+/* DESCRIPTION  : Fonction qui permet d'effectuer la Funny Action en fin de match       */
+/****************************************************************************************/
+void Tourner_modules(void);
+
 Timer t;
 Ticker flipper;
-static char TAB1[25]=   {0x06,0x0A, 0x01, 0xFF, 0x00,                          
-                         0x12,0xBC, 0x02, 0xFF, 0x00,                          
-                         0x04,0xE9, 0x00, 0xFF, 0x00,
-                         0x07,0x4D, 0x01, 0xFF, 0x00,
-                         0x0F,0x90, 0x01, 0xFF, 0x00};                              
+
+///////////TABLEAU PINCE DROITE ///////////////////////////
+static char TAB1[25]=   {0x12,0xA0, 0x00, 0xFF, 0x03,             ///en l'air pince ouverte             
+                         0x04,0x00, 0x02, 0xFF, 0x03,
+                         0x07,0x4D, 0x01, 0xFF, 0x03,
+                         0x0F,0x90, 0x01, 0xFF, 0x03};                              
 
-static char TAB2[25]= {0x06,0x0A, 0x01, 0xFF, 0x00,                          
-                         0x12,0x25, 0x02, 0xFF, 0x00,                              
-                         0x04,0x15, 0x02, 0xFF, 0x00,
-                         0x07,0x4D, 0x01, 0xFF, 0x00,
-                         0x0F,0x90, 0x01, 0xFF, 0x00};
+static char TAB2[25]=   {0x12,0xFF, 0x02, 0xFF, 0x03,               ///en bas pince ouverte               
+                         0x04,0xC0, 0x00, 0xFF, 0x03,
+                         0x07,0x4D, 0x01, 0xFF, 0x03,
+                         0x0F,0x90, 0x01, 0xFF, 0x03};
+                         
+static char TAB3[25]=   {0x12,0xE0, 0x00, 0xFF, 0x03,               ///en haut pince fermee              
+                         0x04,0x15, 0x02, 0xFF, 0x03,
+                         0x07,0xC0, 0x00, 0xFF, 0x03,
+                         0x0F,0x05, 0x02, 0xFF, 0x03}; 
+
+static char TAB4[25]=   {0x12,0x00, 0x01, 0xFF, 0x03,             ///en l'air petit mouvement     
+                         0x04,0x30, 0x01, 0xFF, 0x03,
+                         0x07,0xC0, 0x00, 0xFF, 0x03,
+                         0x0F,0x05, 0x02, 0xFF, 0x03}; 
+                         
+static char TAB5[25]=   {0x12,0xFF, 0x02, 0xFF, 0x03,               ///en bas pince ouverte               
+                         0x04,0xC0, 0x00, 0xFF, 0x03,
+                         0x07,0xC0, 0x00, 0xFF, 0x03,
+                         0x0F,0x05, 0x02, 0xFF, 0x03}; 
+                         
+static char TAB6[25]=   {0x12,0x00, 0x01, 0xFF, 0x03,             ///en l'air fin   
+                         0x04,0x4D, 0x01, 0xFF, 0x03,
+                         0x07,0x4D, 0x01, 0xFF, 0x03,
+                         0x0F,0x90, 0x01, 0xFF, 0x03};     
+                         
                          
-static char TAB3[25]=   {0x06,0x0A, 0x01, 0xFF, 0x00,                         
-                         0x12,0x2C, 0x01, 0xFF, 0x00,                         
-                         0x04,0x15, 0x02, 0xFF, 0x00,
-                         0x07,0x4D, 0x01, 0xFF, 0x00,
-                         0x0F,0x90, 0x01, 0xFF, 0x00}; 
+//////////////////TABLEAU PINCE GAUCHE /////////////////////////////
+static char TAB1125]=   {0x12,0xA0, 0x00, 0xFF, 0x03,             ///en l'air pince ouverte             
+                         0x04,0x00, 0x02, 0xFF, 0x03,
+                         0x07,0x4D, 0x01, 0xFF, 0x03,
+                         0x0F,0x90, 0x01, 0xFF, 0x03};                              
+
+static char TAB12[25]=   {0x12,0xFF, 0x02, 0xFF, 0x03,               ///en bas pince ouverte               
+                         0x04,0xC0, 0x00, 0xFF, 0x03,
+                         0x07,0x4D, 0x01, 0xFF, 0x03,
+                         0x0F,0x90, 0x01, 0xFF, 0x03};
                          
-static char TAB4[25]=   {0x06,0x0A, 0x01, 0xFF, 0x00,                         
-                         0x12,0x2C, 0x01, 0xFF, 0x00,                         
-                         0x04,0x4D, 0x01, 0xFF, 0x00,
-                         0x07,0x4D, 0x01, 0xFF, 0x00,
-                         0x0F,0x90, 0x01, 0xFF, 0x00};  
+static char TAB13[25]=   {0x12,0xE0, 0x00, 0xFF, 0x03,               ///en haut pince fermee              
+                         0x04,0x15, 0x02, 0xFF, 0x03,
+                         0x07,0xC0, 0x00, 0xFF, 0x03,
+                         0x0F,0x05, 0x02, 0xFF, 0x03}; 
+
+static char TAB14[25]=   {0x12,0x00, 0x01, 0xFF, 0x03,             ///en l'air petit mouvement     
+                         0x04,0x30, 0x01, 0xFF, 0x03,
+                         0x07,0xC0, 0x00, 0xFF, 0x03,
+                         0x0F,0x05, 0x02, 0xFF, 0x03}; 
                          
-static char TAB5[25]=   {0x06,0x0A, 0x01, 0xFF, 0x00,                         
-                         0x12,0x2C, 0x01, 0xFF, 0x00,                         
-                         0x04,0x4D, 0x01, 0xFF, 0x00,
-                         0x07,0x6E, 0x01, 0xFF, 0x00,
-                         0x0F,0x90, 0x01, 0xFF, 0x00};        
+static char TAB15[25]=   {0x12,0xFF, 0x02, 0xFF, 0x03,               ///en bas pince ouverte               
+                         0x04,0xC0, 0x00, 0xFF, 0x03,
+                         0x07,0xC0, 0x00, 0xFF, 0x03,
+                         0x0F,0x05, 0x02, 0xFF, 0x03}; 
+                         
+static char TAB1  6[25]=   {0x12,0x00, 0x01, 0xFF, 0x03,             ///en l'air fin   
+                         0x04,0x4D, 0x01, 0xFF, 0x03,
+                         0x07,0x4D, 0x01, 0xFF, 0x03,
+                         0x0F,0x90, 0x01, 0xFF, 0x03};  
                                                       
-
-                        
+                   
                             /*   ANGLE   */
                             
 /*         10° =    0x21, 0x00   |    110°= 0x6E, 0x01    |   210°= 0xBC, 0x02
@@ -61,126 +158,223 @@
            6 =    0x06   |    15 =    0x0F
            7 =    0x07   |    16 =    0x10
            8 =    0x08   |    17 =    0x11                      */
-                                            
-    /*static char Tab1[5]={ 0x09,
-                          0x5E,
-                          0x01,
-                          0x70,
-                          0x01};*/
-    
-    /*static char Tab2[5]={ 0x05,
-                            0x96,
-                            0x01,
-                            0x85,
-                            0x05};*/
-float angle=0.0;
-float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;  
-
-                                                                               
-                            /* DECLARATION PROTOTYPES & POINTEURS */
-                   
-short vitesse=700;                                        // Vitesse angulaire (0-1023)  
-AX12 *dixhuit_myAX12, *quatre_myAX12,*six_myAX12, *sept_myAX12, *quinze_myAX12, *multiple_AX12;
-void Init_AX12(void);
-void Lecture(void);
-void Flip(void);
-
-    
 
 
-                  /*              FONCTIONS                */
+                  
+                                        /* MAIN */                 
 
-void Init_AX12()                                           // Initialisation des différents paramétres
+int main() 
 {
-         
-    dixhuit_myAX12-> Set_Goal_speed(vitesse);             // vitesse (0-1023)
-    quatre_myAX12-> Set_Goal_speed(vitesse);
+    can1.frequency(1000000); // fréquence de travail 1Mbit/s
+    can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
+    
+    CANMessage TrameTx = CANMessage();
+    TrameTx.format=CANStandard;
+    TrameTx.type=CANData;
+    
+    /*
+    deux_myAX12 = new AX12(p9, p10, 2, 1000000);        //main tourneuse
+    quatorze_myAX12 = new AX12(p9, p10, 14, 1000000);   // main tourneuse
+    */
+    
+    trois_myAX12 = new AX12(p9, p10, 3, 1000000);     //pince droite
+    treize_myAX12 = new AX12(p9, p10, 13, 1000000);       //pince droite
+    quatorze_myAX12 = new AX12(p9, p10, 14, 1000000);    //pince droite
+    huit_myAX12 = new AX12(p9, p10, 8, 1000000);   //pince droite
+    
+    quatre_myAX12 = new AX12(p13, p14, 4, 1000000);     //pince gauche
+    sept_myAX12 = new AX12(p13, p14, 7, 1000000);       //pince gauche
+    quinze_myAX12 = new AX12(p13, p14, 15, 1000000);    //pince gauche
+    dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000);   //pince gauche
+    
+    six_myAX12 = new AX12(p28, p27, 6, 1000000);        //funny action
+    
+    multiple_myAX12 = new AX12(p13,p14,0xFE,1000000); 
+    multiple2_myAX12 = new AX12(p9,p10,0xFE,1000000); 
+
+       
+     while(true) {
+        canProcessRx();//Traitement des trames CAN en attente  
+        
+        if (action == 0){
+            choix = 1;
+            Prendre_modules(choix);
+            choix = 2;
+            Prendre_modules(choix);
+            wait(2);
+            /*
+            choix = 1;
+            Poser_modules(choix);
+            choix = 2;
+            Poser_modules(choix);
+            wait(2);
+            
+            TrameTx.id = 0x092;
+            TrameTx.len = 1;
+            TrameTx.data[0] = 0;
+            can1.write(TrameTx);
+            */
+            
+            action = 2;
+            }
+    }
+}    
+
+                        /*              FONCTIONS                */
+    
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx                                                          */
+/* DESCRIPTION  : Fonction de traitement des messages CAN                               */
+/****************************************************************************************/
+void canProcessRx(void)
+{
+    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+    CANMessage msgTx=CANMessage();
+    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+    if(FIFO_occupation<0)
+        FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+    if(FIFO_max_occupation<FIFO_occupation)
+        FIFO_max_occupation=FIFO_occupation;
+    if(FIFO_occupation!=0) {
+        
+        switch(msgRxBuffer[FIFO_lecture].id) {
+            case SERVO_AX12_PROCESS : 
+                action = 1;
+                break;
+                
+            case CHECK_ACTIONNEURS:
+                SendRawId (ALIVE_ACTIONNEURS);
+                break;
+                
+        }
+                    
+        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+    }
+}    
+ 
+/****************************************************************************************/
+/* FUNCTION NAME: canRx_ISR                                                             */
+/* DESCRIPTION  : Interruption en réception sur le CAN                                  */
+/****************************************************************************************/
+void canRx_ISR (void)
+{
+    if (can1.read(msgRxBuffer[FIFO_ecriture])) {
+        if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
+        else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+    }
+} 
+  
+/****************************************************************************************/
+/* FUNCTION NAME: SendRawId                                                             */
+/* DESCRIPTION  : Fonction qui permet d'envoi une trame vide à un ID                    */
+/****************************************************************************************/
+void SendRawId (unsigned short id)
+{
+    CANMessage msgTx=CANMessage();
+    msgTx.id=id;
+    msgTx.len=0;
+    can1.write(msgTx);
+}  
+  void Init_AX12()                                           // Initialisation des différents paramétres
+{
+    deux_myAX12-> Set_Goal_speed(vitesse);             // vitesse (0-1023)
+    quatre_myAX12-> Set_Goal_speed(vitesse);    
     six_myAX12-> Set_Goal_speed(vitesse);
     sept_myAX12-> Set_Goal_speed(vitesse);
+    quatorze_myAX12-> Set_Goal_speed(vitesse);
     quinze_myAX12-> Set_Goal_speed(vitesse);
+    dixhuit_myAX12-> Set_Goal_speed(vitesse);
+    huit_myAX12-> Set_Goal_speed(vitesse);
+    trois_myAX12-> Set_Goal_speed(vitesse);
+    quatorze_myAX12-> Set_Goal_speed(vitesse);
+    treize_myAX12-> Set_Goal_speed(vitesse);
     
-    dixhuit_myAX12->   Set_Mode(0);                          // Mode Position(0) Mode Continu(1)
+    deux_myAX12-> Set_Mode(0);
     quatre_myAX12-> Set_Mode(0);
     six_myAX12-> Set_Mode(0);
     sept_myAX12-> Set_Mode(0);
+    quatorze_myAX12-> Set_Mode(0);
     quinze_myAX12-> Set_Mode(0);
-}                  
-                    
-/*void Lecture()
-{
-    angle= socle_myAX12-> Get_Position();
-    test_socle = socle_myAX12 -> read_and_test(angle,TAB);
-    
-    angle= bas_myAX12-> Get_Position();
-    test_bas = bas_myAX12-> read_and_test(angle,TAB);
-    
-    angle= milieu_myAX12-> Get_Position();
-    test_milieu = milieu_myAX12-> read_and_test(angle,TAB);    
-     
-    angle= haut_myAX12-> Get_Position();
-    test_haut = haut_myAX12-> read_and_test(angle,TAB);
-     
-    angle= ventouse_myAX12-> Get_Position();
-    test_ventouse = ventouse_myAX12-> read_and_test(angle,TAB);
-    
-    valeur_test=test_bas+test_milieu+test_haut+test_socle+test_ventouse;
+    dixhuit_myAX12-> Set_Mode(0); 
+    huit_myAX12-> Set_Mode(0); 
+    trois_myAX12-> Set_Mode(0); 
+    quatorze_myAX12-> Set_Mode(0); 
+    treize_myAX12-> Set_Mode(0); 
+} 
+  
+/****************************************************************************************/
+/* FUNCTION NAME: prendre_modules                                                       */
+/* DESCRIPTION  : Fonction pour prendre des modules lunaires avec la pince              */
+/****************************************************************************************/  
+void Prendre_modules(unsigned char choix){
+    if (choix == 1){
+        //multiple_myAX12->multiple_goal_and_speed(4,TAB1);
+        //wait(TIME);
+        multiple_myAX12->multiple_goal_and_speed(4,TAB2);
+        wait(TIME);
+        multiple_myAX12->multiple_goal_and_speed(4,TAB5);
+        wait(TIME);
+        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(5,TAB6);
+        wait(TIME); 
+    }
     
-    #ifdef AX12_DEBUG
-    printf("\ntest_socle = %f",test_socle);
-    #endif
-    #ifdef AX12_DEBUG
-    printf("\ntest_bas = %f",test_bas);
-    #endif
-    #ifdef AX12_DEBUG
-    printf("\ntest_milieu = %f",test_milieu);
-    #endif
-    #ifdef AX12_DEBUG
-    printf("\ntest_haut = %f",test_haut);
-    #endif
-    #ifdef AX12_DEBUG
-    printf("\ntest_ventouse = %f",test_ventouse);
-    #endif
-     #ifdef AX12_DEBUG
-    printf("\nvaleur_test = %f",valeur_test);
-    #endif    
-}*/
-
-                           /* PROGRAMME PRINCIPAL*/
+    else if (choix == 2){
+        //multiple2_myAX12->multiple_goal_and_speed(4,TAB11);
+        //wait(TIME);
+        multiple2_myAX12->multiple_goal_and_speed(4,TAB12);
+        wait(TIME);
+        multiple2_myAX12->multiple_goal_and_speed(4,TAB15);
+        wait(TIME);
+        multiple2_myAX12->multiple_goal_and_speed(4,TAB13);
+        wait(TIME);
+        multiple2_myAX12->multiple_goal_and_speed(4,TAB14);
+        wait(TIME);  
+        multiple2_myAX12->multiple_goal_and_speed(5,TAB16);
+        wait(TIME); 
+    }
+    
+    SendRawId(0x123);
+}
 
-int main() 
-{
-   // flipper.attach(&Flip, 2.0);
-    dixhuit_myAX12 = new AX12(p13, p14, 18, 1000000);
-    quatre_myAX12 = new AX12(p13, p14, 4, 1000000);
-    six_myAX12 = new AX12(p13, p14, 6, 1000000);
-    sept_myAX12 = new AX12(p13, p14, 7, 1000000);
-    quinze_myAX12 = new AX12(p13, p14, 15, 1000000);
-    multiple_AX12    = new AX12(p13,p14,0xFE,1000000);                           // Création objet : data pin 9&10, ID Broadcast,  baud
-    
-    multiple_AX12->multiple_goal_and_speed(5,TAB1);
-    wait(2);
-    multiple_AX12->multiple_goal_and_speed(5,TAB2);
-    wait(2);
-    multiple_AX12->multiple_goal_and_speed(5,TAB3);
-    wait(2);
-    multiple_AX12->multiple_goal_and_speed(5,TAB4);
-    wait(2);
-    multiple_AX12->multiple_goal_and_speed(5,TAB5);
-    
-    
-    //six_myAX12 -> Set_Secure_Goal(200);
-    /*quatorze_myAX12 -> Set_Secure_Goal(260);
-    gauche_myAX12 -> Set_Secure_Goal(280);    
-    quatorze_myAX12 -> Set_Secure_Goal(300); //avoir ax12 14 a 90°*/
-   
-   /* while(valeur_test<4)
-    {
-        Lecture();
-    }*/
-    
-    
-  
-   
+/****************************************************************************************/
+/* FUNCTION NAME: Poser_modules                                                         */
+/* DESCRIPTION  : Fonction qui permet de déposer des modules lunaires                   */
+/****************************************************************************************/
+void Poser_modules(unsigned char choix){
+    multiple_myAX12->multiple_goal_and_speed(4,TAB4);
+    wait(2);  
+    multiple_myAX12->multiple_goal_and_speed(4,TAB3);
+    wait(2);  
+    multiple_myAX12->multiple_goal_and_speed(4,TAB2);
+    wait(2); 
+    multiple_myAX12->multiple_goal_and_speed(4,TAB1);
+}  
+
+/****************************************************************************************/
+/* FUNCTION NAME: Funny_action                                                          */
+/* DESCRIPTION  : Fonction qui permet d'effectuer la Funny Action en fin de match       */
+/****************************************************************************************/
+void Funny_Action(void){
+    six_myAX12 -> Set_Secure_Goal(150);
+    wait(0.5);
+    six_myAX12 -> Set_Secure_Goal(60);
+    wait(0.5);
+    six_myAX12 -> Set_Secure_Goal(150);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_modules                                                       */
+/* DESCRIPTION  : Fonction qui permet de deployer la main tourneuse                     */
+/****************************************************************************************/
+void Tourner_modules(void){
+    multiple2_myAX12 -> multiple_goal_and_speed(2,TAB11);
+    wait(1.5);
+    multiple2_myAX12 -> multiple_goal_and_speed(2,TAB12);  
+}  
     
    
-}
+