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_small.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_SMALL
#include "Config_small.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) {
}
//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) {
switch(id) {
case 100: // preparation prise bras central
Preparation_prise();
break;
case 101:// stockage haut bras central
Stockage_haut();
break;
case 102:// stockage bas bras central
Stockage_bas();
break;
case 103:// ouvrir la main du bras cental
Deposer();
break;
case 104:// preparation de depot du module bas du bras central
Preparation_depot_bas();
break;
case 105:// preparation de depot du module haut du bras central
Preparation_depot_haut();
break;
case 106:// positionner le bras afin de pousser un module debout
Pousser_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;
} else {
}
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;
} else {
}
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;
} else {
}
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){
//Preparation_module_droit();
}else{
//Preparation_module_gauche();
}
Preparation_module_gauche();
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){
//RangerBrasDroit();
}else{
RangerBrasGauche();
}
break;
case 122:// tourner un module
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){
//Tourner_module_droite();
}else{
Tourner_module_gauche();
}*/
Tourner_module_gauche();
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;
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) {
/**
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();*/
initAX12();
moteurGauchePWM(0);
moteurDroitPWM(0);
}
/****************************************************************************************/
/* FUNCTION NAME: initRobotActionneur */
/* DESCRIPTION : Initialiser la position des actionneurs du robot */
/****************************************************************************************/
void initRobotActionneur(void)
{
/*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
AX12_setGoal(
AX12_ID_BRAS_RELACHEUR_DROIT,
AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_setGoal(
AX12_ID_BRAS_RELACHEUR_GAUCHE,
AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_setGoal(
AX12_ID_BRAS_BASE_DROIT,
AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
AX12_SPEED_BRAS_BASE
);
AX12_setGoal(
AX12_ID_BRAS_BASE_GAUCHE,
AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_setGoal(
AX12_ID_PALET_DROIT,
AX12_ANGLE_PALET_DROIT_FERMER,
AX12_SPEED_PALET
);
AX12_setGoal(
AX12_ID_PALET_GAUCHE,
AX12_ANGLE_PALET_GAUCHE_FERMER,
AX12_SPEED_PALET
);
AX12_processChange(); */
}
/****************************************************************************************/
/* FUNCTION NAME: runTest */
/* DESCRIPTION : tester l'ensemble des actionneurs du robot */
/****************************************************************************************/
void runRobotTest(void)
{
/*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
wait_ms(200);
XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
wait_ms(200);
AX12_setGoal(
AX12_ID_BRAS_BASE_DROIT,
AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
AX12_SPEED_BRAS_BASE
);
AX12_setGoal(
AX12_ID_BRAS_BASE_GAUCHE,
AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
AX12_SPEED_BRAS_BASE
);
AX12_processChange();
wait_ms(500);
AX12_setGoal(
AX12_ID_BRAS_RELACHEUR_DROIT,
AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_setGoal(
AX12_ID_BRAS_RELACHEUR_GAUCHE,
AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_processChange();
wait_ms(600);
AX12_setGoal(
AX12_ID_BRAS_RELACHEUR_DROIT,
AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_setGoal(
AX12_ID_BRAS_RELACHEUR_GAUCHE,
AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_processChange();
wait_ms(500);
AX12_setGoal(
AX12_ID_BRAS_BASE_DROIT,
AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
AX12_SPEED_BRAS_BASE
);
AX12_setGoal(
AX12_ID_BRAS_BASE_GAUCHE,
AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_processChange();
wait_ms(600);
AX12_setGoal(
AX12_ID_PALET_DROIT,
AX12_ANGLE_PALET_DROIT_ROCHET,
AX12_SPEED_PALET
);
AX12_setGoal(
AX12_ID_PALET_GAUCHE,
AX12_ANGLE_PALET_GAUCHE_ROCHET,
AX12_SPEED_PALET
);
AX12_processChange();
wait_ms(500);
AX12_setGoal(
AX12_ID_PALET_DROIT,
AX12_ANGLE_PALET_DROIT_FERMER,
AX12_SPEED_PALET
);
AX12_setGoal(
AX12_ID_PALET_GAUCHE,
AX12_ANGLE_PALET_GAUCHE_FERMER,
AX12_SPEED_PALET
);
AX12_processChange();*/
}
/****************************************************************************************/
/* 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