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-03-31
- Revision:
- 14:c8fc06c4887f
- Parent:
- 12:14729d584500
- Child:
- 15:c2fc239e85df
File content as of revision 14:c8fc06c4887f:
#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://Ouvrir les portes avant
AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE);
AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE);
AX12_processChange();
break;
case 101://Fermer les portes avant
AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER);
AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_FERMER);
AX12_processChange();
break;
case 102://Remonter le peigne
AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP);
SendRawId(0x123);
AX12_processChange();
break;
case 103://Descendre le peigne
AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN);
AX12_processChange();
break;
case 104://Monter le support ventouse haut
AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP);
AX12_processChange();
break;
case 105://Descendre le support ventouse haut
AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_DOWN);
AX12_processChange();
break;
case 106://Remonter le support du cone arriere
AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE);
AX12_processChange();
break;
case 107://Descendre le support du cone arriere
AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE);
AX12_processChange();
break;
case 110://Ouvrir la pince arrière haute
AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 205);
AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 95);
AX12_processChange();
break;
case 111://Fermer la pince arrière haute
AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 145);
AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 155);
AX12_processChange();
/*waitingAckID = AX12_ID_PINCE_ARRIERE_HAUTE_DROITE;
waitingAckFrom = SERVO_AX12_DONE;*/
break;
case 112://Ouvrir la pince arrière basse
AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 215);
AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 85);
AX12_processChange();
break;
case 113://Fermer la pince arrière basse
AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 145);
AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 155);
AX12_processChange();
/* waitingAckID = AX12_ID_PINCE_ARRIERE_BASSE_DROITE;
waitingAckFrom = SERVO_AX12_DONE;*/
break;
case 114://Ouvrir les portes arrières
AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 250);
AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 50);
AX12_processChange();
break;
case 115://Fermer les portes arrière
//AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145);
//AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155);
AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 142);
AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 158);
AX12_processChange();
//waitingAckID = AX12_ID_PORTE_ARRIERE_DROITE;
//waitingAckFrom = SERVO_AX12_DONE;
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);
waitingAckID = ACKNOWLEDGE_TELEMETRE;
waitingAckFrom = 0;
modeTelemetre =1;
case 130://Lancer mouvement de sortie de la zone de départ
SendRawId(ACTION_BIG_DEMARRAGE);
waitingAckID = ACTION_BIG_DEMARRAGE;
waitingAckFrom = INSTRUCTION_END_MOTEUR;
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)
{
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 0x10:
strcpy(cheminFileStart,"/local/moteur.txt");
return FileExists(cheminFileStart);
case 0x11:
strcpy(cheminFileStart,"/local/moteur2.txt");
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