File content as of revision 12:14729d584500:

#include "StrategieManager.h"
#ifdef ROBOT_BIG
#include "Config_big.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) {
    AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
    AX12_processChange();
    
}

/****************************************************************************************/
/* FUNCTION NAME: doAction                                                              */
/* DESCRIPTION  : Effectuer une action specifique                                       */
/****************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
    CANMessage msgTx=CANMessage();
    switch(id) {
        
        case 100://Ouvrir les portes avant
            AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE);
            AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE);
            AX12_processChange();
        break;
        case 101://Fermer les portes avant
            AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER);
            AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_FERMER);
            AX12_processChange();
        break;
        
        case 102://Remonter le peigne
            AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP);
            AX12_processChange();
        break;
        case 103://Descendre le peigne
            AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN);
            AX12_processChange();
        break;
        
        case 104://Monter le support ventouse haut
            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP);
            AX12_processChange();
        break;
        case 105://Descendre le support ventouse haut
            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_DOWN);
            AX12_processChange();
        break;
        case 106://Remonter le support du cone arriere
            AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE);
            AX12_processChange();
        break;
        case 107://Descendre le support du cone arriere
            AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE);
            AX12_processChange();
        break;
        
        case 110://Ouvrir la pince arrière haute
            AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 205);
            AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 95);
            AX12_processChange();
        break;
        case 111://Fermer la pince arrière haute
            AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 145);
            AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 155);
            AX12_processChange();
            /*waitingAckID = AX12_ID_PINCE_ARRIERE_HAUTE_DROITE;
            waitingAckFrom = SERVO_AX12_DONE;*/
        break;
        
        case 112://Ouvrir la pince arrière basse
            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 215);
            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 85);
            AX12_processChange();
        break;
        case 113://Fermer la pince arrière basse
            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 145);
            AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 155);
            AX12_processChange();
           /* waitingAckID = AX12_ID_PINCE_ARRIERE_BASSE_DROITE;
            waitingAckFrom = SERVO_AX12_DONE;*/
        break;
        
        case 114://Ouvrir les portes arrières
            AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 250);
            AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 50);
            AX12_processChange();
        break;
        case 115://Fermer les portes arrière
            //AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145);
            //AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155);
            AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 142);
            AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 158);
            AX12_processChange();
            //waitingAckID = AX12_ID_PORTE_ARRIERE_DROITE;
            //waitingAckFrom = SERVO_AX12_DONE;
        break;
        
        case 120://Activer les pompes
            AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
            AX12_processChange();
        
            
            msgTx.id=POMPE_PWM;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=6;
        
            // x sur 2 octets
            msgTx.data[0]=(unsigned char)POMPES_PWM;
            msgTx.data[1]=(unsigned char)POMPES_PWM;
            msgTx.data[2]=(unsigned char)POMPES_PWM;
            msgTx.data[3]=(unsigned char)POMPES_PWM;
            msgTx.data[4]=(unsigned char)POMPES_PWM;
            msgTx.data[5]=(unsigned char)POMPES_PWM;
        
            can1.write(msgTx);
        break;
        case 121://Désactiver les pompes
            msgTx.id=POMPE_PWM;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=6;
        
            // x sur 2 octets
            msgTx.data[0]=(unsigned char)0;
            msgTx.data[1]=(unsigned char)0;
            msgTx.data[2]=(unsigned char)0;
            msgTx.data[3]=(unsigned char)0;
            msgTx.data[4]=(unsigned char)0;
            msgTx.data[5]=(unsigned char)0;
        
            can1.write(msgTx);
        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);
        break;
        
        case 30://Action tempo
            wait_ms(speed);
        break;
        
        case 130://Lancer mouvement de sortie de la zone de départ
            SendRawId(ACTION_BIG_DEMARRAGE);
            waitingAckID = ACTION_BIG_DEMARRAGE;
            waitingAckFrom = INSTRUCTION_END_MOTEUR;
        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) 
{
    //Enregistrement de tous les AX12 présent sur la carte
    AX12_register(5,  AX12_SERIAL2);
    AX12_register(18, AX12_SERIAL2);
    AX12_register(13, AX12_SERIAL2);
    AX12_register(1,  AX12_SERIAL1);
    AX12_register(11,  AX12_SERIAL1);
    AX12_register(8,  AX12_SERIAL1);
    AX12_register(7,  AX12_SERIAL2);
    
    //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
    //AX12_processChange();
    //runRobotTest();
}

/****************************************************************************************/
/* FUNCTION NAME: initRobotActionneur                                                   */
/* DESCRIPTION  : Initialiser la position des actionneurs du robot                      */
/****************************************************************************************/
void initRobotActionneur(void)
{
    doAction(110,0,0);//Ouverture pince arrière haute
    doAction(112,0,0);//Ouverture pince arrière basse
    doAction(114,0,0);//Ouverture porte arrière
    doAction(100,0,0);//Ouvrir les portes avant
    doAction(102,0,0);//Remonter le peigne
    doAction(106,0,0);//Remonter le support du cone arriere
}

/****************************************************************************************/
/* FUNCTION NAME: runTest                                                               */
/* DESCRIPTION  : tester l'ensemble des actionneurs du robot                            */
/****************************************************************************************/
void runRobotTest(void) 
{
    int waitTime = 500;
    
    //Test des AX12 dans l'ordre
    doAction(111,0,0);//Fermeture pince arrière haute
    wait_ms(waitTime);
    doAction(110,0,0);//Ouverture pince arrière haute
    wait_ms(waitTime);
    doAction(113,0,0);//Fermeture pince arrière basse
    wait_ms(waitTime);
    doAction(112,0,0);//Ouverture pince arrière basse
    wait_ms(waitTime);
    doAction(115,0,0);//Fermeture porte arrière
    wait_ms(waitTime);
    doAction(114,0,0);//Ouverture porte arrière
    wait_ms(waitTime);
    doAction(101,0,0);//Fermer les portes avant
    wait_ms(waitTime);
    doAction(100,0,0);//Ouvrir les portes avant
    wait_ms(waitTime);
    doAction(103,0,0);//Descendre le peigne
    wait_ms(waitTime);
    doAction(102,0,0);//Remonter le peigne
}

/****************************************************************************************/
/* 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);
        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)
{
    doAction(110,0,0);//Ouverture pince arrière haute
    doAction(112,0,0);//Ouverture pince arrière basse
    doAction(114,0,0);//Ouverture porte arrière
    doAction(100,0,0);//Ouvrir les portes avant
    doAction(102,0,0);//Remonter le peigne
}

#endif