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-22
- Revision:
- 18:cc5fec34ed9c
- Parent:
- 16:7321fb3bb396
- Child:
- 19:b4b91258c275
File content as of revision 18:cc5fec34ed9c:
#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) {
//envoie de la funny action
// 0x007, 01, 01
CANMessage msgTx=CANMessage();
msgTx.id=GLOBAL_FUNNY_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=0x01;
msgTx.data[1]=0x01;
can1.write(msgTx);
}
/****************************************************************************************/
/* FUNCTION NAME: doAction */
/* DESCRIPTION : Effectuer une action specifique */
/****************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
CANMessage msgTx=CANMessage();
int localData = 0, localData2;
switch(id) {
case 100: // position initiale
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
msgTx.data[0]=1;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 101: /// preparation prise
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
msgTx.data[0]=2;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 102: // stockage haut
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
msgTx.data[0]=3;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 103: // stockage bas
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
msgTx.data[0]=4;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 104: // pousser module
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
msgTx.data[0]=7;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 105: // preparation prise
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
msgTx.data[0]=6;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 106: //deposer module
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
msgTx.data[0]=5;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 110: // repos bras avant
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
speed = BRAS_AVANT;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
if (speed == BRAS_AVANT)localData = 0x10;
else localData = 0x24;
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 111: // init bras module
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
speed = BRAS_AVANT;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
if (speed == BRAS_AVANT)localData = 0x0A;
else localData = 0x1E;
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 112: // bouger module avant la gouttière
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
speed = BRAS_AVANT;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
if (speed == BRAS_AVANT)localData = 0x0B;
else localData = 0x1F;
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 113: // deposer module
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
localData2 = BRAS_AVANT;
if (InversStrat){ // si on est inversé, on echange les bras
if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
else localData2 = BRAS_AVANT;
}
if(localData2 == BRAS_AVANT){
if(speed == INV){
localData = 0x0E;
}else{
localData = 0x0C;
}
}else{
if(speed == INV){
localData = 0x22;
}else{
localData = 0x20;
}
}
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 114: // repositionner bras avant
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
speed = BRAS_AVANT;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
if (speed == BRAS_AVANT)localData = 0x0F;
else localData = 0x23;
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 115: // pompe avant ON
msgTx.id=0x400;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
localData = BRAS_AVANT;
if (InversStrat){ // si on est inversé, on echange les bras
if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
else localData = BRAS_AVANT;
}
msgTx.data[0]=1;
msgTx.data[1]=localData;
can1.write(msgTx);
wait_ms(1);
can1.write(msgTx);
break;
case 116: // pompe avant OFF
msgTx.id=0x400;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
localData = BRAS_AVANT;
if (InversStrat){ // si on est inversé, on echange les bras
if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
else localData = BRAS_AVANT;
}
msgTx.data[0]=0;
msgTx.data[1]=localData;
can1.write(msgTx);
wait_ms(1);
can1.write(msgTx);
break;
case 120: // repos bras arrière
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
speed = BRAS_ARRIERE;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
if (speed == BRAS_AVANT)localData = 0x10;
else localData = 0x24;
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 121: // position d'init avant de prendre un module bras arriere
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
speed = BRAS_ARRIERE;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
if (speed == BRAS_AVANT)localData = 0xA0;
else localData = 0x1E;
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 122: // bouger le bras arriere avant la gouttiere
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
speed = BRAS_ARRIERE;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
if (speed == BRAS_AVANT)localData = 0x0B;
else localData = 0x1F;
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 123: // deposer module
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
localData2 = BRAS_ARRIERE;
if (InversStrat){ // si on est inversé, on echange les bras
if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
else localData2 = BRAS_AVANT;
}
if(localData2 == BRAS_AVANT){
if(speed == INV){
localData = 0x0E;
}else{
localData = 0x0C;
}
}else{
if(speed == INV){
localData = 0x22;
}else{
localData = 0x20;
}
}
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 124: // remettre le bras arriere en posiion initiale
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
speed = BRAS_ARRIERE;
if (InversStrat){ // si on est inversé, on echange les bras
if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
else speed = BRAS_AVANT;
}
if (speed == BRAS_AVANT)localData = 0x0F;
else localData = 0x23;
msgTx.data[0]=localData;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
case 125: // pompe pwm arriere ON
msgTx.id=0x400;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
localData = BRAS_ARRIERE;
if (InversStrat){ // si on est inversé, on echange les bras
if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
else localData = BRAS_AVANT;
}
msgTx.data[0]=1;
msgTx.data[1]=localData;
can1.write(msgTx);
wait_ms(1);
can1.write(msgTx);
break;
case 126: // pompe arriere pwm OFF
msgTx.id=0x400;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
localData = BRAS_ARRIERE;
if (InversStrat){ // si on est inversé, on echange les bras
if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
else localData = BRAS_AVANT;
}
msgTx.data[0]=0;
msgTx.data[1]=localData;
can1.write(msgTx);
wait_ms(1);
can1.write(msgTx);
break;
case 130: // baiser bras turbine
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=0x14;
msgTx.data[1]=BRAS_AVANT;
can1.write(msgTx);
break;
case 131: // lever bras turbine
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=0x15;
msgTx.data[1]=BRAS_AVANT;
can1.write(msgTx);
break;
case 132: // allumer turbine
msgTx.id=0x403;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=1;
msgTx.data[1]=BRAS_AVANT;
can1.write(msgTx);
break;
case 133: // stop turbine
msgTx.id=0x403;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=0;
msgTx.data[1]=BRAS_AVANT;
can1.write(msgTx);
break;
case 140:// lanceur ON
msgTx.id=0x402;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=1;
msgTx.data[1]=0x02;
can1.write(msgTx);
break;
case 141: // lanceur OFF
msgTx.id=0x402;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=0;
msgTx.data[1]=0x02;
can1.write(msgTx);
break;
case 150:
msgTx.id=0x404;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=1;
msgTx.data[1]=BRAS_AVANT;
can1.write(msgTx);
break;
case 151:
msgTx.id=0x404;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
msgTx.data[0]=0;
msgTx.data[1]=BRAS_AVANT;
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)
{
wait(2);
doAction(101,1,0);// preparation prise pince arriere
doAction(101,0,0);// preparation prise pince avant
doAction(130,0,0);// baisser aspitr
wait(2);
doAction(100,1,0);// preparation prise pince arriere
doAction(100,0,0);// preparation prise pince avant
doAction(131,0,0);// baisser aspitr
}
/****************************************************************************************/
/* 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