File content as of revision 2:9d280856a536:

#include "Actionneur.h"

extern CAN can;       
        
                             /*  DECLARATION VARIABLES */

extern unsigned char FlagAx12;
extern "C" void mbed_reset();//Pour pouvoir reset la carte

extern DigitalOut led2;
extern Serial pc;
extern void GetPositionAx12(void);
extern void gerer_turbine(unsigned char pwm_turbine);
extern PwmOut PompeDroite;
extern unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
                                unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
                                unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3);

static float TAB_ANGLE1[4], TAB_ANGLE2[4];
static char TAB_POSITION[4];
AX12 *deux_myAX12, *trois_myAX12, *quatre_myAX12, *sept_myAX12, *huit_myAX12, *quinze_myAX12, *treize_myAX12, *quatorze_myAX12, *dixhuit_myAX12, *multiple_myAX12, *multiple2_myAX12; 
AX12 *MilieuBrasPompe, *HautBrasPompe, *BasBrasPompe, *BrasPompeAx12, *TabPompeBras;                                 
                                 
                     
                   
                            /*   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 */                 

void initialisation_AX12(void) 
{
    short vitesse=700;
    
    HautBrasPompe = new AX12(p9, p10, 3, 1000000);  
    MilieuBrasPompe = new AX12(p9, p10, 2, 1000000); 
    BasBrasPompe = new AX12(p9, p10, 1, 1000000);
    
    BrasPompeAx12 = new AX12(p9,p10,0xFE,1000000);
    
    HautBrasPompe->Set_Goal_speed(vitesse);  
    MilieuBrasPompe->Set_Goal_speed(vitesse); 
    BasBrasPompe->Set_Goal_speed(vitesse); 
    
    HautBrasPompe->Set_Mode(0); 
    MilieuBrasPompe->Set_Mode(0); 
    BasBrasPompe->Set_Mode(0); 
  
} 

void GetPositionAx12(void) {
    
    pc.printf("\n\r * Bras Pompe * \n\r");
    
    pc.printf("Haut  : %lf \n\r ",   HautBrasPompe->Get_Position()      );
    pc.printf("Milieu : %lf \n\r ",   MilieuBrasPompe->Get_Position()    );
    pc.printf("Bas: %lf \n\r ",   BasBrasPompe->Get_Position() );   
    
}


/****************************************************************************************/
/* FUNCTION NAME: Automate_ax12                                                         */
/* DESCRIPTION  : Fonction qui gère les différentes actions des AX12                    */
/****************************************************************************************/
void AX12_automate(unsigned char etat_ax12, unsigned char choix_bras){
    
    unsigned short speed;
    
    static unsigned char action = 0;
    unsigned int GoalPos1, GoalPos2, GoalPos3;
    switch(etat_ax12){
        
        case AX12_GET_POSITION:
            GetPositionAx12();
            break;
        
        
        case AX12_VIDER_FUSEE_POMPE:
            
            speed=511;
            
            GoalPos1=1500;
            GoalPos2=624;
            GoalPos3=1413;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            GoalPos1+=900;
            GoalPos2=202;
            GoalPos3=2205;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(20*TIME);
            GoalPos3=2557;
            GoalPos2=173;
            GoalPos1=1947;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
            GoalPos3=2557;
            GoalPos2=370;
            GoalPos1=1798;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            GoalPos3=2548;
            GoalPos2=683;
            GoalPos1=1499;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            GoalPos3=2405;
            GoalPos2=1150;
            GoalPos1=1158;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
              
            PompeDroite.write(0);

            Fin_action();          
            break;
            
        case AX12_VIDER_FUSEE_POMPEBLEU:
            
            speed=511;
            
            GoalPos3=2205;
            GoalPos2=202;
            GoalPos1=598;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            GoalPos3=2557;
            GoalPos2=173;
            GoalPos1=1050;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); wait(5*TIME);
            GoalPos3=2557;
            GoalPos2=370;
            GoalPos1=1199;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
            GoalPos3=2548;
            GoalPos2=683;
            GoalPos1=1499;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            GoalPos3=2405;
            GoalPos2=1150;
            GoalPos1=1947;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
            
            PompeDroite.write(0);

            Fin_action();          
            break;
            
         case AX12_REMPLIR_BASE_POMPE:
         
            speed=511;
            
            GoalPos3=2440;
            GoalPos2=302;
            GoalPos1=1950;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            GoalPos3=1865;
            GoalPos2=252;
            GoalPos1=2400;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            GoalPos3=1500;
            GoalPos2=460;
            GoalPos1=2400;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3);
            GoalPos3=1396;
            GoalPos2=600;
            GoalPos1=1399;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            
            GoalPos3=1413;
            GoalPos2=624;
            GoalPos1=1500;
            mvtBrasAvant(1, speed, GoalPos1, 2, speed, GoalPos2, 3, speed, GoalPos3); 
            
            Fin_action();
            break;   
        
        case AX12_INITIALISATION :
            if (FlagAx12 == 1){
                Initialisation_position(1);
                //Check_positionAX12(&TAB1[25], 1);
                Initialisation_position(2);
                //Check_positionAX12(&TAB21[25], 2);
                FlagAx12 = 2;
            }
            else if (choix_bras > 1){
                Initialisation_position(choix_bras);
            }
            break;
                        
        case AX12_PREPARATION_PRISE :
            Preparation_prise(choix_bras);
            if (action == 0){
                Fin_action();
                action ++;
            }
            break;
                        
        case AX12_STOCKAGE_HAUT :
            Stockage_haut(choix_bras);
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_STOCKAGE_BAS :
            Stockage_bas(choix_bras);
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_DEPOSER :
            Deposer(choix_bras);
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_PREPARATION_DEPOT_BAS :
            Preparation_depot_bas(choix_bras);
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_PREPARATION_DEPOT_HAUT :
            Preparation_depot_haut(choix_bras);
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
                        
        case AX12_POUSSER_MODULE :
            Pousser_module(choix_bras);
            etat_ax12 = AX12_DEFAUT;
            Fin_action();
            break;
        
        case AX12_DEFAUT :
        action = 0;
            break;
    }
}
   

unsigned char mvtBrasAvant(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1, 
                                unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
                                unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3)
{
   char TabPompeBras[15];
   unsigned short GPosition1_1, GPosition2_1, GPosition3_1;
   
   GPosition1_1=((unsigned long)GPosition1*341/1000);
   GPosition2_1=((unsigned long)GPosition2*341/1000);
   GPosition3_1=((unsigned long)GPosition3*341/1000);
   
   TabPompeBras[0] = ID1;
   TabPompeBras[1] = GPosition1_1;
   TabPompeBras[2] = GPosition1_1>>8; 
   TabPompeBras[3] = GSpeed1; 
   TabPompeBras[4] = GSpeed1>>8;               /// (haut, milieu, bas)  
             
   TabPompeBras[5] = ID2;
    TabPompeBras[6] = GPosition2_1;
    TabPompeBras[7] = GPosition2_1>>8;
    TabPompeBras[8] = GSpeed2;
    TabPompeBras[9] = GSpeed2>>8;
   
    TabPompeBras[10] = ID3;
    TabPompeBras[11] = GPosition3_1;
    TabPompeBras[12] = GPosition3_1>>8;
    TabPompeBras[13] = GSpeed3;
    TabPompeBras[14] = GSpeed3>>8  ;  
                        
   BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
   wait(TIME);
   
   
   while (((unsigned short)(HautBrasPompe->Get_Position()*10)>GPosition3*105/100) || ((unsigned short)(HautBrasPompe->Get_Position()*10)<GPosition3*95/100)) {
        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
        wait(TIME*5);
    }
    
    while (((unsigned short)(MilieuBrasPompe->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(MilieuBrasPompe->Get_Position()*10)<GPosition2*95/100)) {
        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
        wait(TIME*5);
    }
    
    
    while (((unsigned short)(BasBrasPompe->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(BasBrasPompe->Get_Position()*10)<GPosition1*95/100)) {
        BrasPompeAx12->multiple_goal_and_speed(3,TabPompeBras) ;
        wait(TIME*5);
    }
    
   
    
}

  

/****************************************************************************************/
/* FUNCTION NAME: Initialisation_position                                               */
/* DESCRIPTION  : Fonction qui place les bras en position verticale                     */
/****************************************************************************************/
void Initialisation_position(unsigned char choix){
    if (choix == 1){
        multiple_myAX12->multiple_goal_and_speed(4,TAB1);
        wait(TIME);  
    }
    
    else if (choix == 2){
        multiple2_myAX12->multiple_goal_and_speed(4,TAB21);
        wait(TIME);
    }
    else if (choix == 3){
        BrasPompeAx12->multiple_goal_and_speed(3,TabBrasPompePosInit);
        wait(TIME);
    }
    
    
}

/****************************************************************************************/
/* FUNCTION NAME: Preparation_prise                                                     */
/* DESCRIPTION  : Fonction qui prepare le robot pour prendre les modules                */
/****************************************************************************************/
void Preparation_prise(unsigned char choix){    
    if (choix == 1){
        multiple_myAX12->multiple_goal_and_speed(4,TAB2);
        wait(TIME);
    }
    
    else if (choix == 2){
        multiple2_myAX12->multiple_goal_and_speed(4,TAB22);
        wait(TIME);
    }
}

/****************************************************************************************/
/* FUNCTION NAME: Stockage_haut                                                         */
/* DESCRIPTION  : Fonction qui prend et stocke les modules dans la position haute       */
/****************************************************************************************/
void Stockage_haut(unsigned char choix){
    if (choix == 1){
        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(4,TAB5);
        wait(TIME);
        multiple_myAX12->multiple_goal_and_speed(4,TAB6);
        wait(TIME);
    }
    
    else if (choix == 2){
        multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
        wait(TIME);
        multiple2_myAX12->multiple_goal_and_speed(4,TAB24);
        wait(TIME);
        multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
        wait(TIME);
        multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
        wait(TIME);
    }
}

/****************************************************************************************/
/* FUNCTION NAME: Stockage_bas                                                          */
/* DESCRIPTION  : Fonction qui prend et stocke un module dans la pince                  */
/****************************************************************************************/
void Stockage_bas(unsigned char choix){
    if (choix == 1){
        multiple_myAX12->multiple_goal_and_speed(4,TAB3);
        wait(TIME);
        multiple_myAX12->multiple_goal_and_speed(4,TAB7);
        wait(TIME);
    } 
    
    else if (choix == 2){
        multiple2_myAX12->multiple_goal_and_speed(4,TAB23);
        wait(TIME);
        multiple2_myAX12->multiple_goal_and_speed(4,TAB27);
        wait(TIME);
    }       
}

/****************************************************************************************/
/* FUNCTION NAME: Deposer                                                               */
/* DESCRIPTION  : Fonction qui permet de déposer le module                              */
/****************************************************************************************/
void Deposer(unsigned char choix){
    if (choix == 1){
        multiple_myAX12->multiple_goal_and_speed(4,TAB9);
        wait(TIME);
    }  
    
    else if (choix == 2){
        multiple2_myAX12->multiple_goal_and_speed(4,TAB29);
        wait(TIME);
    }        
}

/****************************************************************************************/
/* FUNCTION NAME: Preparation_depot_bas                                                 */
/* DESCRIPTION  : Fonction qui prépare le depos d'un module en bas                      */
/****************************************************************************************/
void Preparation_depot_bas(unsigned char choix){
    if (choix == 1){
        multiple_myAX12->multiple_goal_and_speed(4,TAB8);
        wait(TIME);
    }
    
    else if (choix == 2){
        multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
        wait(TIME);
    }
}

/****************************************************************************************/
/* FUNCTION NAME: Preparation_depot_haut                                                */
/* DESCRIPTION  : Fonction qui prépare le depos d'un module en haut                     */
/****************************************************************************************/
void Preparation_depot_haut(unsigned char choix){
    if (choix == 1){
        multiple_myAX12->multiple_goal_and_speed(4,TAB6);
        wait(TIME);
        multiple_myAX12->multiple_goal_and_speed(4,TAB5);
        wait(TIME);
        multiple_myAX12->multiple_goal_and_speed(4,TAB10);
        wait(TIME); 
        multiple_myAX12->multiple_goal_and_speed(4,TAB8);
        wait(TIME);   
    }
    
    else if (choix == 2){
        multiple2_myAX12->multiple_goal_and_speed(4,TAB26);
        wait(TIME);
        multiple2_myAX12->multiple_goal_and_speed(4,TAB25);
        wait(TIME);
        multiple2_myAX12->multiple_goal_and_speed(4,TAB30);
        wait(TIME); 
        multiple2_myAX12->multiple_goal_and_speed(4,TAB28);
        wait(TIME);   
    }
}

/****************************************************************************************/
/* FUNCTION NAME: Pousser_module                                                        */
/* DESCRIPTION  : Fonction qui permet pousser le module situé à l'entrée de la bas      */
/****************************************************************************************/
void Pousser_module(unsigned char choix){
    if (choix == 1){
        multiple_myAX12->multiple_goal_and_speed(4,TAB11);
        wait(TIME);   
    }
    
    else if (choix == 2){
        multiple2_myAX12->multiple_goal_and_speed(4,TAB31);
        wait(TIME);   
    }
}

/****************************************************************************************/
/* FUNCTION NAME: Fin_action                                                            */
/* DESCRIPTION  : Fonction qui confirme la fin de mouvement des AX12                    */
/****************************************************************************************/
void Fin_action(void){
    CANMessage msgTx=CANMessage();
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    
    msgTx.id = SERVO_AX12_END;
    msgTx.len = 1;  
    msgTx.data[0] = AX12_PREPARATION_PRISE;
    can.write(msgTx);  
}




/****************************************************************************************/
/* FUNCTION NAME: Check_positionAX12                                                    */
/* DESCRIPTION  : Fonction qui permet de verifier la position des AX12                  */
/****************************************************************************************/
void Check_positionAX12(char* TAB, unsigned char choix){
    int k=1, i=0;
    static float TAB_POS_TH[4];
    
    CANMessage msgTx=CANMessage();
    msgTx.id=SERVO_AX12_POSITION;
    msgTx.len=5;
    
    //PERMET DE VERIFIER LA POSITION D'UN AX12
    TAB_ANGLE1[0] = (unsigned short)(dixhuit_myAX12->Get_Position()/0.3);
    TAB_ANGLE1[1] = (unsigned short)(quatre_myAX12->Get_Position()/0.3);
    TAB_ANGLE1[2] = (unsigned short)(sept_myAX12->Get_Position()/0.3);
    TAB_ANGLE1[3] = (unsigned short)(quinze_myAX12->Get_Position()/0.3);
    TAB_ANGLE2[0] = (unsigned short)(huit_myAX12->Get_Position()/0.3);
    TAB_ANGLE2[1] = (unsigned short)(trois_myAX12->Get_Position()/0.3);
    TAB_ANGLE2[2] = (unsigned short)(treize_myAX12->Get_Position()/0.3);
    TAB_ANGLE2[3] = (unsigned short)(quatorze_myAX12->Get_Position()/0.3);
     
    for(i=0; i<4; i++){
        TAB_POS_TH[i] = (unsigned short) TAB[k] + ((unsigned short)TAB[k+1]<<8);    
        k=k+5;
    }
    
    for(i=0; i<4; i++){
        if (choix == 1){
            if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
                TAB_POSITION[i] = 1;
                }
            else if ((TAB_ANGLE1[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE1[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
                TAB_POSITION[i] = 0;
                }
            }
        else if (choix == 2){
            if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
                TAB_POSITION[i] = 1;
                }
            else if ((TAB_ANGLE2[i] < TAB_POS_TH[i]+TOLERANCE_AX12) && (TAB_ANGLE2[i] > TAB_POS_TH[i]-TOLERANCE_AX12)){
                TAB_POSITION[i] = 0;
                }
            }
     }
    
    msgTx.data[0] = choix;
    for(i=1; i<5; i++){
        msgTx.data[i] = TAB_POSITION[i];
    }
    can.write(msgTx);
}