Strategie_13h30
Fork of CRAC-Strat_2017_homologation_gros_rob by
Robots/Strategie_small.cpp
- Committer:
- matthieuvignon
- Date:
- 2017-05-25
- Revision:
- 22:8dec8824a6f7
- Parent:
- 21:65ba6e4d53fa
File content as of revision 22:8dec8824a6f7:
#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 //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); 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,"/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); case 11: strcpy(cheminFileStart,"/local/grand_8.txt"); return FileExists(cheminFileStart); case 0x10: strcpy(cheminFileStart,"/local/demoBras.txt"); return FileExists(cheminFileStart); default: strcpy(cheminFileStart,"/local/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