main.cpp

00001 /**************************************************************************************************/
00002 /* cette fonction de controle de l'MX12 est réalisée à partir de la même bibliothèque que les AX12*/
00003 /**************************************************************************************************/
00004 
00005 #include "mbed.h"
00006 #include "AX12.h"
00007 #include "cmsis.h"
00008 #include "ident_crac.h"
00009 
00010 #define AX12_INITIALISATION 0
00011 #define AX12_PREPARATION_PRISE 1
00012 #define AX12_STOCKAGE_HAUT 2
00013 #define AX12_STOCKAGE_BAS 3
00014 #define AX12_DEPOSER 4
00015 #define AX12_PREPARATION_DEPOT_BAS 5
00016 #define AX12_PREPARATION_DEPOT_HAUT 6
00017 #define AX12_POUSSER_MODULE 7
00018 #define AX12_DEFAUT 20
00019 
00020 #define TIME 0.8
00021 #define TOLERANCE_AX12 50
00022 #define SIZE_FIFO 25        
00023         
00024                              /*  DECLARATION VARIABLES */
00025 CAN can1(p30,p29);
00026 CANMessage msgRxBuffer[SIZE_FIFO];
00027 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
00028 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
00029 
00030 extern "C" void mbed_reset();//Pour pouvoir reset la carte
00031 
00032 unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0;
00033 static float TAB_ANGLE1[4], TAB_ANGLE2[4];
00034 static char TAB_POSITION[4];
00035 short vitesse=700;
00036 float angle=0.0;
00037 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;  
00038 AX12 *un_myMX12;
00039                                  
00040                                  
00041                     /*       PROTOTYPES DE FONCTIONS ET POINTEURS       */
00042                     
00043   
00044 /****************************************************************************************/
00045 /* FUNCTION NAME: CAN2_wrFilter                                                         */
00046 /* DESCRIPTION  : Fonction qui permet de ne garder que les ID qui nous interessent      */
00047 /****************************************************************************************/                    
00048 void CAN2_wrFilter (uint32_t id);                    
00049                                   
00050 /****************************************************************************************/
00051 /* FUNCTION NAME: canProcessRx                                                          */
00052 /* DESCRIPTION  : Fonction de traitement des messages CAN                               */
00053 /****************************************************************************************/
00054 void canProcessRx(void);
00055 
00056 /****************************************************************************************/
00057 /* FUNCTION NAME: canRx_ISR                                                             */
00058 /* DESCRIPTION  : Interruption en réception sur le CAN                                  */
00059 /****************************************************************************************/
00060 void canRx_ISR (void);
00061 
00062 /****************************************************************************************/
00063 /* FUNCTION NAME: SendRawId                                                             */
00064 /* DESCRIPTION  : Fonction qui permet d'envoi une trame vide à un ID                    */
00065 /****************************************************************************************/
00066 void SendRawId (unsigned short id);
00067 
00068 /****************************************************************************************/
00069 /* FUNCTION NAME: Fin_action                                                            */
00070 /* DESCRIPTION  : Fonction qui confirme la fin de mouvement des AX12                    */
00071 /****************************************************************************************/
00072 void Fin_action(void);
00073 
00074 /****************************************************************************************/
00075 /* FUNCTION NAME: Automate_ax12                                                         */
00076 /* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
00077 /****************************************************************************************/
00078 void AX12_automate(void);
00079 
00080 /****************************************************************************************/
00081 /* FUNCTION NAME: Initialisation                                               */
00082 /* DESCRIPTION  : Fonction qui place les bras en position verticale                     */
00083 /****************************************************************************************/
00084 void Initialisation(void);
00085 
00086 /****************************************************************************************/
00087 /* FUNCTION NAME: bouger_MX12                                                           */
00088 /* DESCRIPTION  : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge    */
00089 /****************************************************************************************/
00090 void bouger_MX12(unsigned char choix);
00091 
00092 Timer t;
00093 Ticker flipper;
00094 
00095                    
00096                             /*   ANGLE   */
00097                             
00098 /*              0 = tourner à droite
00099                 600 = position initiale
00100                 1200 = tourner à gauche
00101                                                              */                   
00102 
00103                             /*  NUMERO MX12 : 0x01 */
00104 
00105                   
00106                                         /* MAIN */                 
00107 
00108 int main() 
00109 {
00110     can1.frequency(1000000); // fréquence de travail 1Mbit/s
00111     can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
00112     CAN2_wrFilter(SERVO_AX12_ACTION); // 0x96 Trie des messages CAN sur les paramètres suivants.
00113     CAN2_wrFilter(SERVO_AX12_ACK);  // 0x106
00114     CAN2_wrFilter(SERVO_AX12_END);  // 0x116
00115     CAN2_wrFilter(CHECK_AX12);      // 0x65
00116 
00117     // Set l'ID du MX12 et la vitesse de communication. Paramètres : tx rx id baud
00118     un_myMX12 = new AX12(p9, p10, 1, 1000000);   //Trappe de fermeture du lanceur.
00119 
00120      while(true) {
00121         AX12_automate();// Trie les actions de l'MX12
00122         canProcessRx(); // Traitement des trames CAN en attente  
00123     }
00124 }    
00125 
00126                         /*              FONCTIONS                */
00127     
00128 /****************************************************************************************/
00129 /* FUNCTION NAME: canProcessRx                                                          */
00130 /* DESCRIPTION  : Fonction de traitement des messages CAN                               */
00131 /****************************************************************************************/
00132 void canProcessRx(void)
00133 {
00134     static signed char FIFO_occupation=0,FIFO_max_occupation=0;
00135     
00136     CANMessage msgTx=CANMessage(); // Set la structure d'un message CAN Tx : id, data, taille, type, format
00137     msgTx.format=CANStandard;
00138     msgTx.type=CANData;
00139     FIFO_occupation=FIFO_ecriture-FIFO_lecture;
00140     
00141     if(FIFO_occupation<0)
00142         FIFO_occupation=FIFO_occupation+SIZE_FIFO; //Taille du CANRx.
00143     
00144     if(FIFO_max_occupation<FIFO_occupation) 
00145         FIFO_max_occupation=FIFO_occupation;
00146     
00147     if(FIFO_occupation!=0) {
00148         
00149         switch(msgRxBuffer[FIFO_lecture].id) {
00150             case CHECK_AX12: // 0x065
00151                 SendRawId(ALIVE_AX12);
00152                 flag = 1;
00153                 break;
00154             
00155             case SERVO_AX12_ACTION : // 0x96
00156                 etat_ax12 = msgRxBuffer[FIFO_lecture].data[0];
00157                 
00158                 //ACK de reception des actions a effectuer
00159                 msgTx.id = SERVO_AX12_ACK;
00160                 msgTx.len = 1;
00161                 msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
00162                 can1.write(msgTx);
00163                 break;
00164         }
00165                     
00166         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
00167     }
00168 }    
00169  
00170 /****************************************************************************************/
00171 /* FUNCTION NAME: canRx_ISR                                                             */
00172 /* DESCRIPTION  : Interruption en réception sur le CAN                                  */
00173 /****************************************************************************************/
00174 void canRx_ISR (void)
00175 {
00176     if (can1.read(msgRxBuffer[FIFO_ecriture])) {
00177         if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
00178         else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
00179     }
00180 } 
00181   
00182 /****************************************************************************************/
00183 /* FUNCTION NAME: SendRawId                                                             */
00184 /* DESCRIPTION  : Fonction qui permet d'envoyer une trame vide à un ID                    */
00185 /****************************************************************************************/
00186 void SendRawId (unsigned short id)
00187 {
00188     CANMessage msgTx=CANMessage();
00189     msgTx.id=id;
00190     msgTx.len=0;
00191     can1.write(msgTx);
00192 }  
00193  
00194 
00195 /****************************************************************************************/
00196 /* FUNCTION NAME: Initialisation                                               */
00197 /* DESCRIPTION  : Place le MX12 en position initiale : lanceur fermé                    */
00198 /****************************************************************************************/
00199 void Initialisation(){
00200     un_myMX12-> Set_Goal_speed(vitesse);
00201     un_myMX12-> Set_Mode(0);
00202 }
00203 
00204 /****************************************************************************************/
00205 /* FUNCTION NAME: bouger_MX12                                                           */
00206 /* DESCRIPTION  : Fonction qui sélectionne le sens de rotation de l'MX12 et le bouge    */
00207 /****************************************************************************************/
00208 void bouger_MX12(unsigned char choix){
00209     AX12 *ptr_myAX12;
00210     ptr_myAX12 = new AX12(p9, p10, 1,1000000); // Create objects
00211     if( choix == 1) // Tourne à droite
00212     {
00213             ptr_myAX12->Set_Secure_Goal(0); // tourner droite
00214     }
00215     else if(choix == 2) // Tourne à gauche
00216     {
00217             ptr_myAX12->Set_Secure_Goal(1200); // tourner gauche
00218     }
00219     else if (choix == 0)
00220     {
00221         ptr_myAX12->Set_Secure_Goal(600); // position initiale
00222     }
00223 }
00224 
00225 /****************************************************************************************/
00226 /* FUNCTION NAME: Fin_action                                                            */
00227 /* DESCRIPTION  : Fonction qui confirme la fin de mouvement des AX12                    */
00228 /****************************************************************************************/
00229 void Fin_action(void){
00230     CANMessage msgTx=CANMessage();
00231     msgTx.format=CANStandard;
00232     msgTx.type=CANData;
00233     
00234     msgTx.id = SERVO_AX12_END;
00235     msgTx.len = 1;  
00236     msgTx.data[0] = AX12_PREPARATION_PRISE;
00237     can1.write(msgTx);  // Envoie le message via le CAN.
00238 }
00239 
00240 /****************************************************************************************/
00241 /* FUNCTION NAME: Automate_ax12                                                         */
00242 /* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
00243 /****************************************************************************************/
00244 void AX12_automate(void){
00245     switch(etat_ax12){
00246         case AX12_INITIALISATION : // Etat = 0
00247             if (flag == 1){ // Décide si il faut initialiser le bras droit ou gauche
00248                 Initialisation();
00249                 flag = 2;
00250             }
00251             break;
00252                         
00253         case AX12_PREPARATION_PRISE : // Etat = 1
00254             bouger_MX12(msgRxBuffer[FIFO_lecture].data[1]);
00255             if (action == 0){
00256                 Fin_action();
00257                 action ++;
00258             }
00259             break;
00260         case AX12_DEFAUT :
00261         action = 0;
00262             break;
00263     }
00264 }
00265 
00266 /****************************************************************************************/
00267 /* FUNCTION NAME: CAN2_wrFilter                                                         */
00268 /* DESCRIPTION  : Fonction qui permet de ne garder que les ID qui nous interessent      */
00269 /****************************************************************************************/
00270 void CAN2_wrFilter (uint32_t id)  {
00271     static int CAN_std_cnt = 0;
00272     uint32_t buf0, buf1;
00273     int cnt1, cnt2, bound1;
00274  
00275     /* Acceptance Filter Memory full */
00276     if (((CAN_std_cnt + 1) >> 1) >= 512)
00277         return;                                       /* error: objects full */
00278  
00279     /* Setup Acceptance Filter Configuration
00280       Acceptance Filter Mode Register = Off  */
00281     LPC_CANAF->AFMR = 0x00000001;
00282  
00283     id |= 1 << 13;                        /* Add controller number(2) */
00284     id &= 0x0000F7FF;                            /* Mask out 16-bits of ID */
00285  
00286     if (CAN_std_cnt == 0)  {                     /* For entering first  ID */
00287         LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
00288     }  else if (CAN_std_cnt == 1)  {             /* For entering second ID */
00289         if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
00290             LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
00291         else
00292             LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
00293     }  else  {
00294         /* Find where to insert new ID */
00295         cnt1 = 0;
00296         cnt2 = CAN_std_cnt;
00297         bound1 = (CAN_std_cnt - 1) >> 1;
00298         while (cnt1 <= bound1)  {                  /* Loop through standard existing IDs */
00299             if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id)  {
00300                 cnt2 = cnt1 * 2;
00301                 break;
00302             }
00303             if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id)  {
00304                 cnt2 = cnt1 * 2 + 1;
00305                 break;
00306             }
00307             cnt1++;                                  /* cnt1 = U32 where to insert new ID */
00308         }                                          /* cnt2 = U16 where to insert new ID */
00309  
00310         if (cnt1 > bound1)  {                      /* Adding ID as last entry */
00311             if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
00312                 LPC_CANAF_RAM->mask[cnt1]  = 0x0000FFFF | (id << 16);
00313             else                                     /* Odd  number of IDs exists */
00314                 LPC_CANAF_RAM->mask[cnt1]  = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
00315         }  else  {
00316             buf0 = LPC_CANAF_RAM->mask[cnt1];        /* Remember current entry */
00317             if ((cnt2 & 0x0001) == 0)                /* Insert new mask to even address */
00318                 buf1 = (id << 16) | (buf0 >> 16);
00319             else                                     /* Insert new mask to odd  address */
00320                 buf1 = (buf0 & 0xFFFF0000) | id;
00321  
00322             LPC_CANAF_RAM->mask[cnt1] = buf1;        /* Insert mask */
00323  
00324             bound1 = CAN_std_cnt >> 1;
00325             /* Move all remaining standard mask entries one place up */
00326             while (cnt1 < bound1)  {
00327                 cnt1++;
00328                 buf1  = LPC_CANAF_RAM->mask[cnt1];
00329                 LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
00330                 buf0  = buf1;
00331             }
00332  
00333             if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
00334                 LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
00335         }
00336     }
00337     CAN_std_cnt++;
00338  
00339     /* Calculate std ID start address (buf0) and ext ID start address <- none (buf1) */
00340     buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
00341     buf1 = buf0;
00342  
00343     /* Setup acceptance filter pointers */
00344     LPC_CANAF->SFF_sa     = 0;
00345     LPC_CANAF->SFF_GRP_sa = buf0;
00346     LPC_CANAF->EFF_sa     = buf0;
00347     LPC_CANAF->EFF_GRP_sa = buf1;
00348     LPC_CANAF->ENDofTable = buf1;
00349  
00350     LPC_CANAF->AFMR = 0x00000000;                  /* Use acceptance filter */
00351 } // CAN2_wrFilter
00352 
00353 /****************************************************************************************/
00354 /* FUNCTION NAME: Check_positionAX12                                                    */
00355 /* DESCRIPTION  : Fonction qui permet de verifier la position de l'MX12                 */
00356 /****************************************************************************************/
00357 void Check_positionAX12(char* TAB, unsigned char choix){
00358     static float TAB_POS_TH[4];
00359     
00360     CANMessage msgTx=CANMessage();
00361     msgTx.id=SERVO_AX12_POSITION;
00362     msgTx.len=5;
00363     
00364     //PERMET DE VERIFIER LA POSITION De l'MX12
00365     TAB_ANGLE1[0] = (unsigned short)(un_myMX12->Get_Position()/0.3);
00366 
00367 
00368         TAB_POS_TH[0] = (unsigned short) TAB[0] + ((unsigned short)TAB[1]<<8);    
00369     
00370         if (choix == 1){
00371             if ((TAB_ANGLE1[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE1[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
00372                 TAB_POSITION[0] = 1;
00373                 }
00374             else if ((TAB_ANGLE1[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE1[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
00375                 TAB_POSITION[0] = 0;
00376                 }
00377             }
00378         else if (choix == 2){
00379             if ((TAB_ANGLE2[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE2[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
00380                 TAB_POSITION[0] = 1;
00381                 }
00382             else if ((TAB_ANGLE2[0] < TAB_POS_TH[0]+TOLERANCE_AX12) && (TAB_ANGLE2[0] > TAB_POS_TH[0]-TOLERANCE_AX12)){
00383                 TAB_POSITION[0] = 0;
00384                 }
00385      }
00386     
00387     msgTx.data[0] = choix;
00388     msgTx.data[0] = TAB_POSITION[0];
00389     can1.write(msgTx);
00390 }            
00391