File content as of revision 17:d1594579eec6:

#include "peripheriques.h"
/* contient les fonctiones qui servent à utiliser les AX12 et les moteurs sur le petit robot*/

PwmOut motGauche(p21);
PwmOut motDroit(p22);

// AX12 partie centrale du petit robot
AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre;
AX12 *partieDoigtCentre;
AX12 *multipleCentre;
// AX12 partie gauche du robot
AX12 *partieBasseGauche, *partieMainGauche;
AX12 *partiePorteGauche;
AX12 *multipleGauche;

// AX12 partie droite du robot
AX12 *partieBasseDroite, *partieMainDroite;
AX12 *partiePorteDroite;
AX12 *multipleDroite;

Serial pc(USBTX, USBRX);

Timer t;
Ticker flipper;
unsigned char action_precedente = 0;


////////////////////// TABLEAU PINCE CENTRALE ///////////////////////////
static char TAB1[20]=   {0x05,0x15, 0x02, 0xFF, 0x00,               ///Position initiale          
                         0x06,0x00, 0x02, 0xFF, 0x00,
                         0x07,0x90, 0x01, 0xB1, 0x00,
                         0x08,0x58, 0x02, 0x79, 0x00};                              

static char TAB2[20]=   {0x05,0x50, 0x00, 0xFF, 0x02,               ///Preparation prise              
                         0x06,0x50, 0x01, 0xFF, 0x02,
                         0x07,0xF4, 0x01, 0xFF, 0x02,
                         0x08,0xF4, 0x01, 0xFF, 0x02};
                         
static char TAB3[20]=   {0x05,0x50, 0x00, 0xFF, 0x03,               ///Stockage haut (pince fermee)             
                         0x06,0x50, 0x01, 0xFF, 0x03,
                         0x07,0xC5, 0x00, 0xFF, 0x03,
                         0x08,0x4D, 0x03, 0xFF, 0x03}; 

static char TAB4[20]=   {0x05,0xA0, 0x01, 0xFF, 0x03,               ///Stockage haut (pince en l'air)            
                         0x06,0x50, 0x01, 0xFF, 0x03,
                         0x07,0xC5, 0x00, 0xFF, 0x03,
                         0x08,0x4D, 0x03, 0xFF, 0x03};
                         
static char TAB5[20]=   {0x05,0xA0, 0x01, 0xFF, 0x03,               ///Stockage haut (module sur tige)            
                         0x06,0xF4, 0x01, 0xFF, 0x00,
                         0x07,0xC5, 0x00, 0xFF, 0x03,
                         0x08,0x4D, 0x03, 0xFF, 0x03}; 
                         
static char TAB6[20]=   {0x05,0xA0, 0x01, 0xFF, 0x03,               ///Stockage haut (pince ouverte)            
                         0x06,0xF4, 0x01, 0xFF, 0x03,
                         0x07,0x4d, 0x01, 0xFF, 0x03,
                         0x08,0x9a, 0x02, 0xFF, 0x03}; 
                         
static char TAB7[20]=   {0x05,0xA0, 0x01, 0xFF, 0x03,               ///Stockage bas (pince en l'air)            
                         0x06,0xB0, 0x00, 0xFF, 0x03,
                         0x07,0xC5, 0x00, 0xFF, 0x00,
                         0x08,0x4D, 0x03, 0xFF, 0x00}; 
                         
static char TAB8[20]=   {0x05,0x40, 0x00, 0xFF, 0x03,               ///Preparation_depot_bas            
                         0x06,0xF4, 0x01, 0xFF, 0x03,
                         0x07,0xC5, 0x00, 0xFF, 0x00,
                         0x08,0x4D, 0x03, 0xFF, 0x00}; 
                         
static char TAB9[20]=   {0x05,0x40, 0x00, 0xFF, 0x03,               ///Deposer         
                         0x06,0xF4, 0x01, 0xFF, 0x03,
                         0x07,0xD0, 0x00, 0xFF, 0x00,
                         0x08,0x35, 0x03, 0xFF, 0x00}; 
                         
static char TAB10[20]=   {0x05,0xA0, 0x01, 0xFF, 0x03,               ///Stockage haut (module sur tige)            
                         0x06,0x00, 0x01, 0xFF, 0x00,
                         0x07,0xC5, 0x00, 0xFF, 0x03,
                         0x08,0x4D, 0x03, 0xFF, 0x03};  
                         
static char TAB11[20]=   {0x05,0x60, 0x00, 0xFF, 0x03,               ///Pousser_module            
                         0x06,0xF4, 0x01, 0xFF, 0x03,
                         0x07,0xC5, 0x00, 0xFF, 0x00,
                         0x08,0x4D, 0x03, 0xFF, 0x00};       
                      
static char TAB12[20]=   {0x05,0x05, 0x02, 0xFF, 0x03,               ///Sortie position initiale        
                         0x06,0x00, 0x02, 0xFF, 0x03,
                         0x07,0xF4, 0x01, 0xFF, 0x03,
                         0x08,0xF4, 0x01, 0xFF, 0x03}; 
                         
static char TAB13[20]=   {0x05,0xF4, 0x00, 0xFF, 0x03,               ///Deposer         
                         0x06,0xA0, 0x02, 0xFF, 0x03,
                         0x07,0xD0, 0x00, 0xFF, 0x00,
                         0x08,0x35, 0x03, 0xFF, 0x00};                                                                           

static char TAB14[20]=   {0x05,0x15, 0x02, 0xFF, 0x03,               ///Stockage haut (pince ouverte)            
                         0x06,0x42, 0x00, 0xFF, 0x03,
                         0x07,0x4d, 0x01, 0xFF, 0x03,
                         0x08,0x9a, 0x02, 0xFF, 0x03}; 

static char TAB15[20]=   {0x05,0x15, 0x02, 0xFF, 0x03,               ///Stockage haut (module sur tige)            
                         0x06,0x42, 0x00, 0xFF, 0x00,
                         0x07,0xC5, 0x00, 0xFF, 0x03,
                         0x08,0x4D, 0x03, 0xFF, 0x03}; 
                         
static char TAB16[20]=   {0x05,0x15, 0x02, 0xFF, 0x03,               ///Stockage haut (pince ouverte)            
                         0x06,0x42, 0x00, 0xFF, 0x00,
                         0x07,0x2c, 0x01, 0xFF, 0x03,
                         0x08,0x4D, 0x03, 0xFF, 0x03}; 
////////////////////// TABLEAU BRAS GAUCHE ///////////////////////////
static char TAB21[10]=   {0x01,0x50, 0x03, 0xFF, 0x03,               ///Position initiale          
                         0x02,0xF4, 0x01, 0xFF, 0x03};    
                         
static char TAB22[10]=   {0x01,0x20, 0x01, 0xFF, 0x03,               ///Preparation_tourner        
                         0x02,0x40, 0x03, 0xFF, 0x03};   
                    
static char TAB23[10]=   {0x01,0x20, 0x01, 0xFF, 0x03,               ///Tourner_module         
                         0x02,0xE5, 0x02, 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                      */
           
           
           
           
                         
void declarationAX12(void){
    //Pince centrale
    partieBasseCentre = new AX12(p9, p10, 5, 1000000);   
    partieMainGaucheCentre = new AX12(p9, p10, 7, 1000000);   
    partieMainDroiteCentre = new AX12(p9, p10, 8, 1000000);  
    partiePoignetCentre = new AX12(p9, p10, 6, 1000000);   
    //doigt central
    partieDoigtCentre =  new AX12(p9, p10, 4, 1000000);
    
    multipleCentre = new AX12(p9,p10,0xFE,1000000);  
    
    //Bras de gauche
    partieBasseGauche = new AX12(p13, p14, 1, 1000000);
    partieMainGauche = new AX12(p13, p14, 2, 1000000);
    //Porte gauche
    partiePorteGauche = new AX12(p13, p14, 3, 1000000);
    
    multipleGauche = new AX12(p13,p14,0xFE,1000000);

    //Bras de droite
    partieBasseDroite = new AX12(p28, p27, 1, 1000000);
    partieMainDroite = new AX12(p28, p27, 2, 1000000);
    //Porte droite
    partiePorteDroite = new AX12(p28, p27, 3, 1000000);

    multipleDroite = new AX12(p28,p27,0xFE,1000000);
    
    
    
    
    
    }
    
    
void initAX12()
{
    declarationAX12();
    
    // init des AX12 de la partie CENTRALE du robot
    partieBasseCentre-> Set_Goal_speed(VITESSE);             // VITESSE (0-1023)
    
    partieMainGauche-> Set_Goal_speed(VITESSE);    
    partieMainDroite-> Set_Goal_speed(VITESSE);
    partiePoignetCentre-> Set_Goal_speed(VITESSE);
    partieDoigtCentre-> Set_Goal_speed(VITESSE);
    
    partieBasseCentre-> Set_Mode(0);
    partieMainGauche-> Set_Mode(0);
    partieMainDroite-> Set_Mode(0);
    partiePoignetCentre-> Set_Mode(0);
    partieDoigtCentre-> Set_Mode(0);
    
    // init des AX12 de ela partie GAUCHE du robot
    partieBasseGauche-> Set_Goal_speed(VITESSE);
    partieMainGauche-> Set_Goal_speed(VITESSE);
    partiePorteGauche-> Set_Goal_speed(VITESSE);
    
    partieBasseGauche-> Set_Mode(0);
    partieMainGauche-> Set_Mode(0);
    partiePorteGauche-> Set_Mode(0);
    
    // init des AX12 de ela partie DROITE du robot
    partieBasseDroite-> Set_Goal_speed(VITESSE);
    partieMainDroite-> Set_Goal_speed(VITESSE);
    //partiePorteDroite-> Set_Goal_speed(VITESSE);
    
    partieBasseDroite-> Set_Mode(0);
    partieMainDroite-> Set_Mode(0);
    //partiePorteDroite-> Set_Mode(0);
    
    
} 
  

/****************************************************************************************/
/* FUNCTION NAME: Initialisation_position                                               */
/* DESCRIPTION  : Fonction qui place les bras en position verticale                     */
/****************************************************************************************/
void Initialisation_position(void){
    multipleCentre->multiple_goal_and_speed(5,TAB12);
    wait(TIME);
    multipleCentre->multiple_goal_and_speed(5,TAB1);
    //wait(TIME); 
    multipleGauche->multiple_goal_and_speed(3,TAB21);
    wait(TIME); 
}

/****************************************************************************************/
/* FUNCTION NAME: Preparation_prise                                                     */
/* DESCRIPTION  : Fonction qui prepare le robot pour prendre les modules                */
/****************************************************************************************/
void Preparation_prise(void){    
    /*if (action_precedente == 0){
        multipleCentre->multiple_goal_and_speed(5,TAB12);
        wait(TIME);
        action_precedente = 1;
    }*/
    multipleCentre->multiple_goal_and_speed(5,TAB2);                                                           
    wait(TIME);
}

/****************************************************************************************/
/* FUNCTION NAME: Stockage_haut                                                         */
/* DESCRIPTION  : Fonction qui prend et stocke les modules dans la position haute       */
/****************************************************************************************/
void Stockage_haut(void){
    multipleCentre->multiple_goal_and_speed(5,TAB3);
    wait(TIME);
    multipleCentre->multiple_goal_and_speed(5,TAB4);
    wait(TIME);
    multipleCentre->multiple_goal_and_speed(5,TAB5);
    wait(TIME);
    multipleCentre->multiple_goal_and_speed(5,TAB6);
    wait(TIME);
}

/****************************************************************************************/
/* FUNCTION NAME: Stockage_bas                                                          */
/* DESCRIPTION  : Fonction qui prend et stocke un module dans la pince                  */
/****************************************************************************************/
void Stockage_bas(void){
    multipleCentre->multiple_goal_and_speed(5,TAB3);
    wait(TIME);
    multipleCentre->multiple_goal_and_speed(5,TAB7);
    wait(TIME);
}

/****************************************************************************************/
/* FUNCTION NAME: Deposer                                                               */
/* DESCRIPTION  : Fonction qui permet de déposer le module                              */
/****************************************************************************************/
void Deposer(void){
    multipleCentre->multiple_goal_and_speed(5,TAB9);
    wait(TIME/5);
    multipleCentre->multiple_goal_and_speed(5,TAB8);
    wait(TIME/5);
    //multipleCentre->multiple_goal_and_speed(5,TAB13);
    wait(TIME/5);
}

/****************************************************************************************/
/* FUNCTION NAME: Preparation_depot_bas                                                 */
/* DESCRIPTION  : Fonction qui prépare le depos d'un module en bas                      */
/****************************************************************************************/
void Preparation_depot_bas(void){
    multipleCentre->multiple_goal_and_speed(5,TAB8);
    wait(TIME);
}

/****************************************************************************************/
/* FUNCTION NAME: Preparation_depot_haut                                                */
/* DESCRIPTION  : Fonction qui prépare le depos d'un module en haut                     */
/****************************************************************************************/
void Preparation_depot_haut(void){
    multipleCentre->multiple_goal_and_speed(5,TAB14);
    wait(TIME);
    multipleCentre->multiple_goal_and_speed(5,TAB16);
    wait(TIME);
    multipleCentre->multiple_goal_and_speed(5,TAB15);
    wait(TIME);
    multipleCentre->multiple_goal_and_speed(5,TAB10);
    wait(TIME); 
    multipleCentre->multiple_goal_and_speed(5,TAB8);
    wait(TIME);   
}

/****************************************************************************************/
/* FUNCTION NAME: Pousser_module                                                        */
/* DESCRIPTION  : Fonction qui permet pousser le module situé à l'entrée de la bas      */
/****************************************************************************************/
void Pousser_module(void){
    multipleCentre->multiple_goal_and_speed(5,TAB11);
    wait(TIME);   
}

/****************************************************************************************/
/* FUNCTION NAME: Initialisation_gauche                                                 */
/* DESCRIPTION  : Fonction qui permet de placer le cote gauche en position initiale     */
/****************************************************************************************/
void Initialisation_gauche(void){
    //trois_myAX12_2->Set_Secure_Goal(235);
    multipleGauche->multiple_goal_and_speed(5,TAB22);
    wait(TIME);
    multipleGauche->multiple_goal_and_speed(5,TAB21);
    wait(TIME);   
}

/****************************************************************************************/
/* FUNCTION NAME: Preparation_prise_gauche                                              */
/* DESCRIPTION  : Fonction qui permet de preparer la recuperation d'un module           */
/****************************************************************************************/
void Preparation_prise_gauche(void){
    //trois_myAX12_2->Set_Secure_Goal(120);
    //multiplePorteGauche->multiple_goal_and_speed(5,TAB);
}

/****************************************************************************************/
/* FUNCTION NAME: Prendre_module_gauche                                                 */
/* DESCRIPTION  : Fonction qui permet prendre un module sur le cote gauche              */
/****************************************************************************************/
void Prendre_module_gauche(void){
    //trois_myAX12_2->Set_Secure_Goal(160);
    //multiplePorteGauche->multiple_goal_and_speed(5,TAB);
}

/***************************************************************************************/
/* FUNCTION NAME: RangerBrasGauche                                                     */
/* DESCRIPTION  : Fonction range le bras gauche                                         */
/****************************************************************************************/
void RangerBrasGauche(void){
    multipleGauche->multiple_goal_and_speed(3,TAB21);
    wait(TIME); 
}

/****************************************************************************************/
/* FUNCTION NAME: Preparation_module_gauche                                             */
/* DESCRIPTION  : Fonction qui prepare la tournante                                     */
/****************************************************************************************/
void Preparation_module_gauche(void){
    multipleGauche->multiple_goal_and_speed(5,TAB22);
    wait(TIME);
}

/****************************************************************************************/
/* FUNCTION NAME: Tourner_module_gauche                                                 */
/* DESCRIPTION  : Fonction qui permet de tourner les modules a gauche                   */
/****************************************************************************************/
void Tourner_module_gauche(void){
    //#define POSITION_TOURNER 0xE5
    //#define POSITION_MAX 0xFF
    /*int position = POSITION_TOURNER;
    TAB23[6] = POSITION_TOURNER;*/
    
    //multipleGauche->multiple_goal_and_speed(5,TAB23);
    wait(TIME);
    /*while((dataPressionGauche == false)&&(position < POSITION_MAX)){
        position++;
        TAB23[6] = position;
        multipleGauche->multiple_goal_and_speed(5,TAB23);
        }*/
        
    while(dataCouleurGauche() == false){
            printf("ici");
            moteurDroitPWM(0.2);
        } 
    moteurDroitPWM(0);
} 


void getPosiotionCentrale(void){
    pc.printf("\n\r * pince avant * \n\r");
    
    pc.printf("base  : %lf \n\r ",   partieBasseCentre->Get_Position()      );
    pc.printf("coude : %lf \n\r ",   partiePoignetCentre->Get_Position()    );
    pc.printf("doigtG: %lf \n\r ",   partieMainGaucheCentre->Get_Position() );
    pc.printf("doigtD: %lf \n\n\r ", partieMainDroiteCentre->Get_Position() );
    
    wait(0.2);
}

void getPosiotionGauche(void){
    pc.printf("\n\r * bras gauche *\n\r");
    
    pc.printf("epaule : %lf \n\r ",partieBasseGauche->Get_Position());
    pc.printf("main   : %lf \n\r ",partieMainGauche->Get_Position() );
    pc.printf("porte  : %lf \n\r ",partiePorteGauche->Get_Position() );
    
    wait(0.2);
}

void getPosiotionDroite(void){
    pc.printf("\n\r * bras droit *\n\r");
    
    pc.printf("epaule : %lf \n\r ",partieBasseDroite->Get_Position());
    pc.printf("main   : %lf \n\r ",partieMainDroite->Get_Position() );
    pc.printf("porte  : %lf \n\r ",partiePorteDroite->Get_Position() );
    
    
    wait(0.2);
}




/********************************************************************************************************/


/*********************************************************************************************************/
/* FUNCTION NAME: moteurGauchePWM                                                                        */
/* DESCRIPTION  : bouge le moteur gauche                                                                 */
/*********************************************************************************************************/
void moteurGauchePWM(float pwm){
    motGauche.write(pwm); 
}

/*********************************************************************************************************/
/* FUNCTION NAME: moteurDroitPWM                                                                        */
/* DESCRIPTION  : bouge le moteur gauche                                                                 */
/*********************************************************************************************************/
void moteurDroitPWM(float pwm){
    motDroit.write(pwm); 
}

/*********************************************************************************************************/
/* FUNCTION NAME: initMoteurs                                                                            */
/* DESCRIPTION  : init les moteurs des mains du petit robot                                              */
/*********************************************************************************************************/
void initMoteurs(void){
    motGauche.period(T_MOT);
    motDroit.period(T_MOT);
    motGauche.write(0.0);
    motDroit.write(0.0);
}