AX12 gros robot

Fork of command_AX12_gros_robot by CRAC Team

main.cpp

Committer:
SquirrelGod
Date:
2017-04-18
Revision:
2:99b1cb0d9f5e
Parent:
1:b3ff77670606
Child:
3:6cbd7d2c78f4

File content as of revision 2:99b1cb0d9f5e:

#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;

///////////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]=   {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};     
                         
                         
//////////////////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 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 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
           20° =    0x42, 0x00   |    120°= 0x90, 0x01    |   220°= 0xDD, 0x02
           30° =    0x64, 0x00   |    130°= 0xB1, 0x01
           40° =    0x85, 0x00   |    140°= 0xD2, 0x01
           50° =    0xA6, 0x00   |    150°= 0xF4, 0x01
           60° =    0xC8, 0x00   |    160°= 0x15, 0x02
           70° =    0xE9, 0x00   |    170°= 0x36, 0x02
           80° =    0x0A, 0x01   |    180°= 0x58, 0x02
           90° =    0x2C, 0x01   |    190°= 0x79, 0x02
           100°=    0x4D, 0x01   |    200°= 0x9A, 0x02                         */                   

                            /*  NUMERO AX12  */
                            
/*         0 =    0x00   |    9  =    0x09  |  18 =   0x12
           1 =    0x01   |    10 =    0x0A 
           2 =    0x02   |    11 =    0x0B
           3 =    0x03   |    12 =    0x0C
           4 =    0x04   |    13 =    0x0D
           5 =    0x05   |    14 =    0x0E
           6 =    0x06   |    15 =    0x0F
           7 =    0x07   |    16 =    0x10
           8 =    0x08   |    17 =    0x11                      */


                  
                                        /* MAIN */                 

int main() 
{
    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);
    
    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);
    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); 
    }
    
    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);
}

/****************************************************************************************/
/* 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);  
}