v2019
Dependencies: CRAC-Strat_2019 SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Dependents: Codeprincipal_2019 CRAC-Strat_2019
Robots/Strategie_big.cpp
- Committer:
- ClementBreteau
- Date:
- 2017-05-19
- Revision:
- 16:7321fb3bb396
- Parent:
- 15:c2fc239e85df
- Child:
- 18:cc5fec34ed9c
File content as of revision 16:7321fb3bb396:
#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