File content as of revision 29:41e02746041d:

#include "global.h"
#ifdef ROBOT_SMALL


unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
//unsigned short telemetreDistance;


/****************************************************************************************/
/* FUNCTION NAME: doFunnyAction                                                         */
/* DESCRIPTION  : Permet de faire la funny action en fin de partie                      */
/****************************************************************************************/
void doFunnyAction(void) {
    
    
}
//L'angle est entre 0 et 1023
void XL320_setGoal(unsigned char id, unsigned short angle);

void XL320_setGoal(unsigned char id, unsigned short angle)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=SERVO_XL320;
    msgTx.len=3;
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    msgTx.data[0]=(unsigned char)id;
    // from sur 2 octets
    msgTx.data[1]=(unsigned char)angle;
    msgTx.data[2]=(unsigned char)(angle>>8);

    can1.write(msgTx);
}


/****************************************************************************************/
/* FUNCTION NAME: doAction                                                              */
/* DESCRIPTION  : Effectuer une action specifique                                       */
/****************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
    int retour = 1;
    switch(id) {
        case 100: // preparation prise bras central
            AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_PRISE);
        break;
        case 101:// stockage haut bras central
            AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_HAUT);
        break;
        case 102:// stockage bas bras central
            AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_BAS);
        break;
        case 103:// ouvrir la main du bras cental 
            AX12_automate(AX12_PINCE_CENTRALE_DEPOSER);
        break;
        case 104:// preparation de depot du module bas du bras central
            AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
        break;
        case 105:// preparation de depot du module haut du bras central
            AX12_automate(AX12_PINCE_CENTRALE_DEPOT_HAUT);
            AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
        break;
        case 106:// on rentre la pince dans le robot
            AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
        break;
        case 107: // on ferme la pince du robot
            AX12_automate(AX12_PINCE_CENTRALE_PRISE_MODULE);
        break;
        
        case 110:// ouvrir une porte avant
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                if(speed == 1) speed = 0;
                else speed = 1;
            }
            
            if (speed == 1){
                AX12_automate(AX12_DROIT_CROC_OUVERT);
            }else{
                AX12_automate(AX12_GAUCHE_CROC_OUVERT);    
            }
        break;
        case 111:// securiser un module avec une porte avant
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                if(speed == 1) speed = 0;
                else speed = 1;
            }
            
            if (speed == 1){
                AX12_automate(AX12_DROIT_CROC_FERME);
            }else{
                AX12_automate(AX12_GAUCHE_CROC_FERME);    
            }
        break;
        case 112:// ranger le porte avant dans le robot
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                if(speed == 1) speed = 0;
                else speed = 1;
            }
            
            if (speed == 1){
                AX12_automate(AX12_DROIT_CROC_INITIALE);
            }else{
                AX12_automate(AX12_GAUCHE_CROC_INITIALE);    
            }
        break;
        case 120:// poser une main tourneuse
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                if(speed == 2) speed = 1;
                else speed = 2;
                if(angle == 1) angle = 0;
                else angle = 1;
            }
            
            if (speed == 1){
                AX12_automate(AX12_TOURNANTE_DROIT_PREPARATION);
            }else{
                AX12_automate(AX12_TOURNANTE_GAUCHE_PREPARATION);    
            }
        break;
        
        case 121:// relever une main tourneuse
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                if(speed == 2) speed = 1;
                else speed = 2;
                if(angle == 1) angle = 0;
                else angle = 1;
            }
            
            if (speed == 1){
                AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
            }else{
                AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);    
            }
            
        break;
        
        case 122:// tourner un module
            if(InversStrat == 1) {//Si c'est inversé
                if(speed == 2) speed = 1;
                else speed = 2;
                if(angle == 1) angle = 0;
                else angle = 1;
            }
            
            if (speed == 1){
                AX12_automate(AX12_TOURNANTE_DROIT_MODULE);
                Tourner_module_droit();
            }else{
                AX12_automate(AX12_TOURNANTE_GAUCHE_MODULE);    
                Tourner_module_gauche();
            }
        break;
        
        case 200 :
            telemetreDistance = dataTelemetre();
            wait_ms(1);
            telemetreDistance = dataTelemetre();
            telemetreDistance = telemetreDistance - 170;
        break;
        
        case 201 :
            SendRawId(0x96);
            retour = 2;
        break;
        
        case 10://Désactiver le stop
            isStopEnable = 0;
        break;
        case 11://Activer le stop
            isStopEnable = 1;
        break;
        case 20://Désactiver l'asservissement
            setAsservissementEtat(0);
        break;
        case 21://Activer l'asservissement
            setAsservissementEtat(1);
        break; 
        
        case 22://Changer la vitesse du robot
            SendSpeed(speed,(unsigned short)angle);
            wait_us(200);
            waitingAckFrom = 0;
            waitingAckID = 0;
        break;
        
        case 19: // CHANGER LA VITESSE + DECELERATION 
            SendSpeedDecel(speed,(unsigned short) angle);
            wait_us(200);
            waitingAckFrom = 0;
            waitingAckID =0;
        break;
        
        case 30://Action tempo
            wait_ms(speed);
        break;
        
        default:
            retour = 0;//L'action n'existe pas, il faut utiliser le CAN
        
    }
    return retour;//L'action est spécifique.
    
}

/****************************************************************************************/
/* FUNCTION NAME: initRobot                                                             */
/* DESCRIPTION  : initialiser le robot                                                  */
/****************************************************************************************/
void initRobot(void) {
    /**
    On enregistre les id des AX12 présent sur la carte
    **/
    /*AX12_register(1,AX12_SERIAL1,0x0FF);
    AX12_register(2,AX12_SERIAL1);
    AX12_register(18,AX12_SERIAL1);
    AX12_register(4,AX12_SERIAL2);
    AX12_register(16,AX12_SERIAL2);
    AX12_register(17,AX12_SERIAL2,0x0FF);*/
    
    //runRobotTest();
    
    /*
    AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
    AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
    AX12_setGoal(AX12_ID_BRAS_BASE,278,0x0FF);
    AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras
    AX12_processChange();*/
    
    initialisation_AX12();
    
}

/****************************************************************************************/
/* FUNCTION NAME: initRobotActionneur                                                   */
/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
/****************************************************************************************/
void initRobotActionneur(void)
{
    moteurGauchePWM(0);
    moteurDroitPWM(0);
    AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
    AX12_automate(AX12_GAUCHE_CROC_INITIALE);
    AX12_automate(AX12_DROIT_CROC_INITIALE);
    AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);
    AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
}

/****************************************************************************************/
/* FUNCTION NAME: runTest                                                               */
/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
/****************************************************************************************/
void runRobotTest(void) 
{

}

/****************************************************************************************/
/* FUNCTION NAME: SelectStrategy                                                        */
/* DESCRIPTION  : Charger le fichier de stratégie correspondante à un id                */
/* RETURN       : 0=> Erreur, 1=> OK si le fichier existe                               */
/****************************************************************************************/
int SelectStrategy(unsigned char id)
{
    
    switch(id)
    {
        case 1:
            strcpy(cheminFileStart,"/sd/strat1.txt");
            return FileExists(cheminFileStart);
        case 2:
            strcpy(cheminFileStart,"/sd/strat2.txt");
            return FileExists(cheminFileStart);
        case 3:
            strcpy(cheminFileStart,"/sd/strat3.txt");
            return FileExists(cheminFileStart);
        case 4:
            strcpy(cheminFileStart,"/sd/strat4.txt");
            return FileExists(cheminFileStart);
        case 5:
            strcpy(cheminFileStart,"/sd/strat5.txt");
            return FileExists(cheminFileStart);
        case 6:
            strcpy(cheminFileStart,"/sd/strat6.txt");
            return FileExists(cheminFileStart);
        case 7:
            strcpy(cheminFileStart,"/sd/strat7.txt");
            return FileExists(cheminFileStart);
        case 8:
            strcpy(cheminFileStart,"/sd/strat8.txt");
            return FileExists(cheminFileStart);
        case 9:
            strcpy(cheminFileStart,"/sd/strat9.txt");
            return FileExists(cheminFileStart);
        case 10:
            strcpy(cheminFileStart,"/sd/strat10.txt");
            return FileExists(cheminFileStart);
        case 11:
            strcpy(cheminFileStart,"/sd/grand_8.txt");
            return FileExists(cheminFileStart);
        
        case 0x10:
            strcpy(cheminFileStart,"/sd/demoBras.txt");
            return FileExists(cheminFileStart);
        
        default:
            strcpy(cheminFileStart,"/sd/strat1.txt");
            SendRawId(0x258);
            return 0;
    }
}

/****************************************************************************************/
/* FUNCTION NAME: needToStop                                                            */
/* DESCRIPTION  : Savoir si il faut autoriser le stop du robot via balise               */
/****************************************************************************************/
unsigned char needToStop(void)
{
    return isStopEnable;
}

/****************************************************************************************/
/* FUNCTION NAME: doBeforeEndAction                                                     */
/* DESCRIPTION  : Terminer les actions du robot 1s avant la fin du match                */
/****************************************************************************************/
void doBeforeEndAction(void)
{
    
}

#endif