main.cpp

00001 #include "mbed.h"
00002 #include "AX12.h"
00003 #include "actionneurs.h"
00004 #include "global.h"
00005 
00006 #include "ident_crac.h"
00007 
00008 #define AX12_INITIALISATION 0
00009 #define AX12_PREPARATION_PRISE 1
00010 #define AX12_STOCKAGE_HAUT 2
00011 #define AX12_STOCKAGE_BAS 3
00012 #define AX12_DEPOSER 4
00013 #define AX12_PREPARATION_DEPOT_BAS 5
00014 #define AX12_PREPARATION_DEPOT_HAUT 6
00015 #define AX12_POUSSER_MODULE 7
00016 #define AX12_DEFAUT 20
00017 
00018 
00019 #define SIZE_FIFO 25     
00020 
00021 
00022 extern "C" void mbed_reset();//Pour pouvoir reset la carte
00023 
00024 /****************************************************************************************/
00025 /* FUNCTION NAME: canProcessRx                                                          */
00026 /* DESCRIPTION  : Fonction de traitement des messages CAN                               */
00027 /****************************************************************************************/
00028 void canProcessRx(void);
00029 
00030 /****************************************************************************************/
00031 /* FUNCTION NAME: canRx_ISR                                                             */
00032 /* DESCRIPTION  : Interruption en réception sur le CAN                                  */
00033 /****************************************************************************************/
00034 void canRx_ISR (void);
00035 
00036 /****************************************************************************************/
00037 /* FUNCTION NAME: SendRawId                                                             */
00038 /* DESCRIPTION  : Fonction qui permet d'envoi une trame vide à un ID                    */
00039 /****************************************************************************************/
00040 void SendRawId (unsigned short id);
00041 
00042 /****************************************************************************************/
00043 /* FUNCTION NAME: Fin_action                                                            */
00044 /* DESCRIPTION  : Fonction qui confirme la fin de mouvement des AX12                    */
00045 /****************************************************************************************/
00046 void Fin_action(void);
00047 
00048 /****************************************************************************************/
00049 /* FUNCTION NAME: Automate_ax12                                                         */
00050 /* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
00051 /****************************************************************************************/
00052 void AX12_automate(void);
00053    
00054         
00055                              /*  DECLARATION VARIABLES */
00056 CAN can1(p30,p29);
00057 CANMessage msgRxBuffer[SIZE_FIFO];
00058 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
00059 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
00060 
00061 extern "C" void mbed_reset();//Pour pouvoir reset la carte
00062 
00063 unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0;
00064 short vitesse=700;
00065 float angle=0.0;
00066 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;
00067  
00068                   
00069                                         /* MAIN */                 
00070 
00071 int main() 
00072 {
00073     can1.frequency(1000000); // fréquence de travail 1Mbit/s
00074     can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
00075 
00076     declarationAX12(); 
00077 
00078     //Demonstration bras gauche
00079     /*Initialisation_gauche();
00080     wait(1);
00081     Preparation_prise_gauche();
00082     wait(1);
00083     Prendre_module_gauche();
00084     wait(1);
00085     Preparation_module_gauche();
00086     wait(1);
00087     Tourner_module_gauche();
00088     wait(1);
00089     Preparation_module_gauche();
00090     wait(1);
00091     Tourner_module_gauche();
00092     wait(1);
00093     Preparation_module_gauche();
00094     wait(1);
00095     Tourner_module_gauche();*/
00096     float position =0;
00097      while(true) {
00098         //AX12_automate();
00099         canProcessRx();
00100         
00101     }
00102 }    
00103 
00104                         /*              FONCTIONS                */
00105     
00106 /****************************************************************************************/
00107 /* FUNCTION NAME: canProcessRx                                                          */
00108 /* DESCRIPTION  : Fonction de traitement des messages CAN                               */
00109 /****************************************************************************************/
00110 void canProcessRx(void)
00111 {
00112     static signed char FIFO_occupation=0,FIFO_max_occupation=0;
00113     
00114     CANMessage msgTx=CANMessage();
00115     msgTx.format=CANStandard;
00116     msgTx.type=CANData;
00117     FIFO_occupation=FIFO_ecriture-FIFO_lecture;
00118     
00119     if(FIFO_occupation<0)
00120         FIFO_occupation=FIFO_occupation+SIZE_FIFO;
00121     
00122     if(FIFO_max_occupation<FIFO_occupation)
00123         FIFO_max_occupation=FIFO_occupation;
00124     
00125     if(FIFO_occupation!=0) {
00126         
00127         switch(msgRxBuffer[FIFO_lecture].id) {
00128             case CHECK_AX12:
00129                 SendRawId(ALIVE_AX12);
00130                 flag = 1;
00131                 mbed_reset();
00132                 break;
00133             
00134             case SERVO_AX12_ACTION : 
00135                 etat_ax12 = msgRxBuffer[FIFO_lecture].data[0];
00136                 
00137                 //ACK de reception des actions a effectuer
00138                 msgTx.id = SERVO_AX12_ACK;
00139                 msgTx.len = 1;
00140                 msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
00141                 can1.write(msgTx);
00142                 break;
00143             case 0x123:
00144                 SendRawId(ALIVE_AX12);
00145                 if(msgRxBuffer[FIFO_lecture].data[0] == 0){
00146                     getPosiotionCentrale();
00147                 }else if(msgRxBuffer[FIFO_lecture].data[0] == 1){
00148                     getPosiotionGauche();
00149                 }else{
00150                     getPosiotionDroite();
00151                 }
00152                 break;
00153             case 0x456:
00154                 //Demonstration pince centrale
00155                 switch(msgRxBuffer[FIFO_lecture].data[0]){
00156                 case 0:
00157                     Initialisation_position();
00158                     break;
00159                 case 1:
00160                     Preparation_prise();
00161                     break;
00162                 case 2:
00163                     Stockage_haut();
00164                     break;
00165                 case 3:
00166                     Stockage_bas();
00167                     break;
00168                 case 4:
00169                     Pousser_module();
00170                     break;
00171                 case 5:
00172                     Preparation_depot_bas();
00173                     break;
00174                 case 6:
00175                     Deposer();
00176                     break;
00177                 case 7:
00178                     Preparation_depot_haut();
00179                     break;
00180                 
00181                 }
00182                 wait(1);
00183                 break;
00184         }
00185                     
00186         FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
00187     }
00188 }    
00189  
00190 /****************************************************************************************/
00191 /* FUNCTION NAME: canRx_ISR                                                             */
00192 /* DESCRIPTION  : Interruption en réception sur le CAN                                  */
00193 /****************************************************************************************/
00194 void canRx_ISR (void)
00195 {
00196     if (can1.read(msgRxBuffer[FIFO_ecriture])) {
00197         if(msgRxBuffer[FIFO_ecriture].id==0x65) mbed_reset();
00198         else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
00199     }
00200 } 
00201   
00202 /****************************************************************************************/
00203 /* FUNCTION NAME: SendRawId                                                             */
00204 /* DESCRIPTION  : Fonction qui permet d'envoi une trame vide à un ID                    */
00205 /****************************************************************************************/
00206 void SendRawId (unsigned short id)
00207 {
00208     CANMessage msgTx=CANMessage();
00209     msgTx.id=id;
00210     msgTx.len=0;
00211     can1.write(msgTx);
00212 }  
00213   
00214 
00215 
00216 /****************************************************************************************/
00217 /* FUNCTION NAME: Fin_action                                                            */
00218 /* DESCRIPTION  : Fonction qui confirme la fin de mouvement des AX12                    */
00219 /****************************************************************************************/
00220 void Fin_action(void){
00221     CANMessage msgTx=CANMessage();
00222     msgTx.format=CANStandard;
00223     msgTx.type=CANData;
00224     
00225     msgTx.id = SERVO_AX12_END;
00226     msgTx.len = 1;  
00227     msgTx.data[0] = AX12_PREPARATION_PRISE;
00228     can1.write(msgTx);  
00229 }
00230 
00231 /****************************************************************************************/
00232 /* FUNCTION NAME: Automate_ax12                                                         */
00233 /* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
00234 /****************************************************************************************/
00235 void AX12_automate(void){
00236     switch(etat_ax12){
00237         case AX12_INITIALISATION :
00238             if (flag == 1){
00239                 Initialisation_position();
00240                 flag = 2;
00241             }
00242             
00243             break;
00244                         
00245         case AX12_PREPARATION_PRISE :
00246             Preparation_prise();
00247             if (action == 0){
00248                 Fin_action();
00249                 action ++;
00250             }
00251             break;
00252                         
00253         case AX12_STOCKAGE_HAUT :
00254             Stockage_haut();
00255             etat_ax12 = AX12_DEFAUT;
00256             Fin_action();
00257             break;
00258                         
00259         case AX12_STOCKAGE_BAS :
00260             Stockage_bas();
00261             etat_ax12 = AX12_DEFAUT;
00262             Fin_action();
00263             break;
00264                         
00265         case AX12_DEPOSER :
00266             Deposer();
00267             etat_ax12 = AX12_DEFAUT;
00268             Fin_action();
00269             break;
00270                         
00271         case AX12_PREPARATION_DEPOT_BAS :
00272             Preparation_depot_bas();
00273             etat_ax12 = AX12_DEFAUT;
00274             Fin_action();
00275             break;
00276                         
00277         case AX12_PREPARATION_DEPOT_HAUT :
00278             Preparation_depot_haut();
00279             etat_ax12 = AX12_DEFAUT;
00280             Fin_action();
00281             break;
00282                         
00283         case AX12_POUSSER_MODULE :
00284             Pousser_module();
00285             etat_ax12 = AX12_DEFAUT;
00286             Fin_action();
00287             break;
00288         
00289         case AX12_DEFAUT :
00290         action = 0;
00291             break;
00292     }
00293 }