File content as of revision 12:14729d584500:

#include "StrategieManager.h"
#ifdef ROBOT_SMALL
#include "Config_small.h"

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

/****************************************************************************************/
/* 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) {
    switch(id) {
        case 101://Descendre le bras pour pecher les poissons
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                AX12_setGoal(
                    AX12_ID_BRAS_BASE_DROIT,
                    AX12_ANGLE_BRAS_BASE_DROIT_OUVERT,
                    AX12_SPEED_BRAS_BASE
                );
                AX12_setGoal(
                    AX12_ID_BRAS_RELACHEUR_DROIT,
                    AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
                    AX12_SPEED_BRAS_RELACHEUR
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_BASE_DROIT;
                waitingAckFrom = SERVO_AX12_DONE;
            } else {
                AX12_setGoal(
                    AX12_ID_BRAS_BASE_GAUCHE,
                    AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT,
                    AX12_SPEED_BRAS_BASE
                );
                AX12_setGoal(
                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
                    AX12_SPEED_BRAS_RELACHEUR
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
                waitingAckFrom = SERVO_AX12_DONE;
            }
        break;
        case 102://Remonter le bras avec les poissons dessus
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                AX12_setGoal(
                    AX12_ID_BRAS_BASE_DROIT,
                    AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
                    AX12_SPEED_BRAS_BASE
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_BASE_DROIT;
                waitingAckFrom = SERVO_AX12_DONE;
            } else {
                AX12_setGoal(
                    AX12_ID_BRAS_BASE_GAUCHE,
                    AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
                    AX12_SPEED_BRAS_BASE
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
                waitingAckFrom = SERVO_AX12_DONE;
            }
        break;
        case 103://Decendre à 30° 
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                AX12_setGoal(
                    AX12_ID_BRAS_BASE_DROIT,
                    AX12_ANGLE_BRAS_BASE_DROIT_RELACHER,
                    AX12_SPEED_BRAS_BASE
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_BASE_DROIT;
                waitingAckFrom = SERVO_AX12_DONE;
            } else {
                AX12_setGoal(
                    AX12_ID_BRAS_BASE_GAUCHE,
                    AX12_ANGLE_BRAS_BASE_GAUCHE_RELACHER,
                    AX12_SPEED_BRAS_BASE
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
                waitingAckFrom = SERVO_AX12_DONE;
            }
        break;
        case 104://ouvrir séparateur
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                AX12_setGoal(
                    AX12_ID_BRAS_RELACHEUR_DROIT,
                    AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
                    AX12_SPEED_BRAS_RELACHEUR
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_RELACHEUR_DROIT;
                waitingAckFrom = SERVO_AX12_DONE;
            } else {
                AX12_setGoal(
                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
                    AX12_SPEED_BRAS_RELACHEUR
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_RELACHEUR_GAUCHE;
                waitingAckFrom = SERVO_AX12_DONE;
            }
        break;
        case 105://Rentrer le bras dans le robot, fermer le séparateur
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                AX12_setGoal(
                    AX12_ID_BRAS_RELACHEUR_DROIT,
                    AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
                    AX12_SPEED_BRAS_RELACHEUR
                );
                AX12_processChange();
                wait_ms(100);
                AX12_setGoal(
                    AX12_ID_BRAS_BASE_DROIT,
                    AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
                    AX12_SPEED_BRAS_BASE
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_BASE_DROIT;
                waitingAckFrom = SERVO_AX12_DONE;
            } else { 
                AX12_setGoal(
                    AX12_ID_BRAS_RELACHEUR_GAUCHE,
                    AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
                    AX12_SPEED_BRAS_RELACHEUR
                );
                AX12_processChange();
                wait_ms(100);
                AX12_setGoal(
                    AX12_ID_BRAS_BASE_GAUCHE,
                    AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
                    AX12_SPEED_BRAS_RELACHEUR
                );
                AX12_processChange();
                waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
                waitingAckFrom = SERVO_AX12_DONE;
            }
        break;
        case 106://descendre l'attrape coquillages au niveau des rochets  gauche si non inversé, droit si inversé
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                AX12_setGoal(
                    AX12_ID_PALET_DROIT,
                    AX12_ANGLE_PALET_DROIT_ROCHET,
                    AX12_SPEED_PALET
                );
                AX12_processChange();
                waitingAckID = AX12_ID_PALET_DROIT;
                waitingAckFrom = SERVO_AX12_DONE;
            } else {
                AX12_setGoal(
                    AX12_ID_PALET_GAUCHE,
                    AX12_ANGLE_PALET_GAUCHE_ROCHET,
                    AX12_SPEED_PALET
                );
                AX12_processChange();
                waitingAckID = AX12_ID_PALET_GAUCHE;
                waitingAckFrom = SERVO_AX12_DONE;
            }
        break;
        case 107://descendre l'attrape coquillages au niveau de la table  gauche si non inversé, droit si inversé
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                AX12_setGoal(
                    AX12_ID_PALET_DROIT,
                    AX12_ANGLE_PALET_DROIT_OUVERT,
                    AX12_SPEED_PALET
                );
                AX12_processChange();
                waitingAckID = AX12_ID_PALET_DROIT;
                waitingAckFrom = SERVO_AX12_DONE;
            } else {
                AX12_setGoal(
                    AX12_ID_PALET_GAUCHE,
                    AX12_ANGLE_PALET_GAUCHE_OUVERT,
                    AX12_SPEED_PALET
                );
                AX12_processChange();
                waitingAckID = AX12_ID_PALET_GAUCHE;
                waitingAckFrom = SERVO_AX12_DONE;
            }
        break;
        case 108://Remonter l'attrape coquillages   gauche si non inversé, droit si inversé
            if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
                AX12_setGoal(
                    AX12_ID_PALET_DROIT,
                    AX12_ANGLE_PALET_DROIT_FERMER,
                    AX12_SPEED_PALET
                );
                AX12_processChange();
                waitingAckID = AX12_ID_PALET_DROIT;
                waitingAckFrom = SERVO_AX12_DONE;
            } else {
                AX12_setGoal(
                    AX12_ID_PALET_GAUCHE,
                    AX12_ANGLE_PALET_GAUCHE_FERMER,
                    AX12_SPEED_PALET
                );
                AX12_processChange();
                waitingAckID = AX12_ID_PALET_GAUCHE;
                waitingAckFrom = SERVO_AX12_DONE;
            }
        break;
        
        
        case 200://ouvir la pince gauche
            if(InversStrat == 1) {
                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
            } else {
                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
            }
        break;
        case 205://Sécuriser le palet gauche
            if(InversStrat == 1) {
                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
            } else {
                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
            }
        break;
        case 202://fermer pince gauche
            if(InversStrat == 1) {
                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
            } else {
                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
            }
        break;
        
        case 201://ouvir la pince droite
            if(InversStrat == 1) {
                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
            } else {
                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
            }
        break;
        case 204://Sécuriser le palet droit
            if(InversStrat == 1) {
                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
            } else {
                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
            }
        break;
        case 203://fermer pince droite
            if(InversStrat == 1) {
                XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
            } else {
                XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
            }
        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;
        
        default:
            return 0;//L'action n'existe pas, il faut utiliser le CAN
        
    }
    return 1;//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();*/
}

/****************************************************************************************/
/* FUNCTION NAME: initRobotActionneur                                                   */
/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
/****************************************************************************************/
void initRobotActionneur(void)
{
    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
    
    AX12_setGoal(
        AX12_ID_BRAS_RELACHEUR_DROIT,
        AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
        AX12_SPEED_BRAS_RELACHEUR
    );
    AX12_setGoal(
        AX12_ID_BRAS_RELACHEUR_GAUCHE,
        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
        AX12_SPEED_BRAS_RELACHEUR
    );
    AX12_setGoal(
        AX12_ID_BRAS_BASE_DROIT,
        AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
        AX12_SPEED_BRAS_BASE
    );
    AX12_setGoal(
        AX12_ID_BRAS_BASE_GAUCHE,
        AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
        AX12_SPEED_BRAS_RELACHEUR
    );
    AX12_setGoal(
        AX12_ID_PALET_DROIT,
        AX12_ANGLE_PALET_DROIT_FERMER,
        AX12_SPEED_PALET
    );
    AX12_setGoal(
        AX12_ID_PALET_GAUCHE,
        AX12_ANGLE_PALET_GAUCHE_FERMER,
        AX12_SPEED_PALET
    );
    AX12_processChange();
}

/****************************************************************************************/
/* FUNCTION NAME: runTest                                                               */
/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
/****************************************************************************************/
void runRobotTest(void) 
{
    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
    
    wait_ms(200);
    
    XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
    XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
    
    wait_ms(200);
    
    AX12_setGoal(
        AX12_ID_BRAS_BASE_DROIT,
        AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
        AX12_SPEED_BRAS_BASE
    );
    AX12_setGoal(
        AX12_ID_BRAS_BASE_GAUCHE,
        AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
        AX12_SPEED_BRAS_BASE
    );
    AX12_processChange();
        
    wait_ms(500);        
    
    AX12_setGoal(
        AX12_ID_BRAS_RELACHEUR_DROIT,
        AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
        AX12_SPEED_BRAS_RELACHEUR
    );
    AX12_setGoal(
        AX12_ID_BRAS_RELACHEUR_GAUCHE,
        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
        AX12_SPEED_BRAS_RELACHEUR
    );
    AX12_processChange();
    
    wait_ms(600);
    
    AX12_setGoal(
        AX12_ID_BRAS_RELACHEUR_DROIT,
        AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
        AX12_SPEED_BRAS_RELACHEUR
    );
    AX12_setGoal(
        AX12_ID_BRAS_RELACHEUR_GAUCHE,
        AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
        AX12_SPEED_BRAS_RELACHEUR
    );
    AX12_processChange();
    
    wait_ms(500);
    
    AX12_setGoal(
        AX12_ID_BRAS_BASE_DROIT,
        AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
        AX12_SPEED_BRAS_BASE
    );
    AX12_setGoal(
        AX12_ID_BRAS_BASE_GAUCHE,
        AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
        AX12_SPEED_BRAS_RELACHEUR
    );
    AX12_processChange();
    
    wait_ms(600);
    AX12_setGoal(
        AX12_ID_PALET_DROIT,
        AX12_ANGLE_PALET_DROIT_ROCHET,
        AX12_SPEED_PALET
    );
    AX12_setGoal(
        AX12_ID_PALET_GAUCHE,
        AX12_ANGLE_PALET_GAUCHE_ROCHET,
        AX12_SPEED_PALET
    );
    AX12_processChange();
    
    wait_ms(500);
    
    AX12_setGoal(
        AX12_ID_PALET_DROIT,
        AX12_ANGLE_PALET_DROIT_FERMER,
        AX12_SPEED_PALET
    );
    AX12_setGoal(
        AX12_ID_PALET_GAUCHE,
        AX12_ANGLE_PALET_GAUCHE_FERMER,
        AX12_SPEED_PALET
    );
    AX12_processChange();
}

/****************************************************************************************/
/* 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,"/local/strat1.txt");
            return FileExists(cheminFileStart);
        case 2:
            strcpy(cheminFileStart,"/local/strat2.txt");
            return FileExists(cheminFileStart);
        case 3:
            strcpy(cheminFileStart,"/local/strat3.txt");
            return FileExists(cheminFileStart);
        case 4:
            strcpy(cheminFileStart,"/local/strat4.txt");
            return FileExists(cheminFileStart);
        case 5:
            strcpy(cheminFileStart,"/local/strat5.txt");
            return FileExists(cheminFileStart);
        case 6:
            strcpy(cheminFileStart,"/local/strat6.txt");
            return FileExists(cheminFileStart);
        case 10:
            strcpy(cheminFileStart,"/local/grand_8.txt");
            return FileExists(cheminFileStart);
        default:
            strcpy(cheminFileStart,"/local/strat1.txt");
            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