CRAC Team / CRAC-Strat_2017_fin_premier_match

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of CRAC-Strat_2017_homologation_petit_rob by CRAC Team

Robots/Strategie_big.cpp

Committer:
ClementBreteau
Date:
2017-05-11
Revision:
15:c2fc239e85df
Parent:
14:c8fc06c4887f
Child:
16:7321fb3bb396

File content as of revision 15:c2fc239e85df:


#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://preparation prise
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=1;
            if (InversStrat){ // si on est inversé, on echange les bras
                    if (speed == BRAS_GAUCHE) speed = BRAS_DROIT;
                    else speed = BRAS_GAUCHE;
                }
            msgTx.data[1]=speed;
            can1.write(msgTx);
        break;
        case 101://stockage haut
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=2;
            msgTx.data[1]=speed;
        
            can1.write(msgTx);
        break;
        
        case 102://stockage bas
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=3;
            msgTx.data[1]=speed;
        
            can1.write(msgTx);
        break;
        case 103://deposer
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=4;
            msgTx.data[1]=speed;
        
            can1.write(msgTx);
        break;
        
        case 104://preparation depot bas
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=5;
            msgTx.data[1]=speed;
        
            can1.write(msgTx);
        break;
        case 105://preparation depot haut
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=6;
            msgTx.data[1]=speed;
        
            can1.write(msgTx);
        break;
        case 106://pousser module
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=7;
            msgTx.data[1]=speed;
        
            can1.write(msgTx);
        break;
        
        case 110://Ouvrir la pince arrière haute
           msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=10;
            msgTx.data[1]=speed;
        
            can1.write(msgTx);
        break;
        case 111://Fermer la pince arrière haute
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=3;
        
            // action et le cote selectionné
            msgTx.data[0]=11;
            msgTx.data[1]=speed;
            msgTx.data[2]=angle;
        
            can1.write(msgTx);
        break;
        
        case 112://Ouvrir la pince arrière basse
            msgTx.id=SERVO_AX12_ACTION;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=2;
        
            // action et le cote selectionné
            msgTx.data[0]=12;
            msgTx.data[1]=speed;
        
            can1.write(msgTx);
        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 40: // demande au telemetre la position d'un objet
            //SendRawId(TELEMETRE_RECHERCHE_OBJET);
            
            modeTelemetre = 1;
           
            //angle = angle /10;
            
            msgTx.id=TELEMETRE_OBJET;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=1;
            // indice du module sur le terrain
            msgTx.data[0] = (unsigned char)speed;
            
            /*
            // x sur 2 octets
            msgTx.data[0]=(unsigned char)speed;
            msgTx.data[1]=(unsigned char)(speed>>8);
            // y sur 2 octets
            msgTx.data[2]=(unsigned char)angle;
            msgTx.data[3]=(unsigned char)(angle>>8);
            // theta signé sur 2 octets
            //msgTx.data[4]=(unsigned char)theta;
            //msgTx.data[5]=(unsigned char)(theta>>8);
            msgTx.data[4]=0;
            msgTx.data[5]=0;*/
            
            can1.write(msgTx);
            
        break;
        
        case 130://Lancer mouvement de sortie de la zone de départ
            msgTx.id=ACTION_BIG_DEMARRAGE;
            msgTx.format=CANStandard;
            msgTx.type=CANData;
            msgTx.len=1;
            msgTx.data[0] = (unsigned char)speed;
            can1.write(msgTx);
        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)
    {
        // strat de match
        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 7:
            strcpy(cheminFileStart,"/local/strat7.txt");
            return FileExists(cheminFileStart);
        case 8:
            strcpy(cheminFileStart,"/local/strat8.txt");
            return FileExists(cheminFileStart);
        case 9:
            strcpy(cheminFileStart,"/local/strat9.txt");
            return FileExists(cheminFileStart);
        case 10:
            strcpy(cheminFileStart,"/local/strat10.txt");
            return FileExists(cheminFileStart);
        
        // strat de demo
        case 0x10:
            strcpy(cheminFileStart,"/local/moteur.txt");
            return FileExists(cheminFileStart);
        case 0x11:
#ifdef ROBOT_BIG
            strcpy(cheminFileStart,"/local/bras.txt");
#else
            strcpy(cheminFileStart,"/local/porteAvant.txt");
#endif
            return FileExists(cheminFileStart);
        case 0x12:
#ifdef ROBOT_BIG
            strcpy(cheminFileStart,"/local/balancier.txt");
#else
            strcpy(cheminFileStart,"/local/mainTourneuse.txt");
#endif
            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