CRAC Team
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Robots/Strategie_small.cpp
- Revision:
- 12:14729d584500
- Parent:
- 11:ed13a480ddca
- Child:
- 15:c2fc239e85df
--- a/Robots/Strategie_small.cpp Mon May 02 19:40:59 2016 +0000
+++ b/Robots/Strategie_small.cpp Mon May 09 09:10:17 2016 +0000
@@ -2,6 +2,8 @@
#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 */
@@ -41,6 +43,11 @@
AX12_ANGLE_BRAS_BASE_DROIT_OUVERT,
AX12_SPEED_BRAS_BASE
);
+ AX12_setGoal(
+ AX12_ID_BRAS_RELACHEUR_DROIT,
+ AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+ AX12_SPEED_BRAS_RELACHEUR
+ );
AX12_processChange();
waitingAckID = AX12_ID_BRAS_BASE_DROIT;
waitingAckFrom = SERVO_AX12_DONE;
@@ -50,6 +57,11 @@
AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT,
AX12_SPEED_BRAS_BASE
);
+ AX12_setGoal(
+ AX12_ID_BRAS_RELACHEUR_GAUCHE,
+ AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+ AX12_SPEED_BRAS_RELACHEUR
+ );
AX12_processChange();
waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
waitingAckFrom = SERVO_AX12_DONE;
@@ -121,27 +133,31 @@
case 105://Rentrer le bras dans le robot, fermer le séparateur
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
AX12_setGoal(
+ AX12_ID_BRAS_RELACHEUR_DROIT,
+ AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
+ AX12_SPEED_BRAS_RELACHEUR
+ );
+ AX12_processChange();
+ wait_ms(100);
+ AX12_setGoal(
AX12_ID_BRAS_BASE_DROIT,
AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
AX12_SPEED_BRAS_BASE
);
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_DROIT,
- AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
AX12_processChange();
waitingAckID = AX12_ID_BRAS_BASE_DROIT;
waitingAckFrom = SERVO_AX12_DONE;
} else {
AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
+ AX12_ID_BRAS_RELACHEUR_GAUCHE,
+ AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
AX12_SPEED_BRAS_RELACHEUR
);
+ AX12_processChange();
+ wait_ms(100);
AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_GAUCHE,
- AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
+ AX12_ID_BRAS_BASE_GAUCHE,
+ AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
AX12_SPEED_BRAS_RELACHEUR
);
AX12_processChange();
@@ -216,9 +232,9 @@
case 200://ouvir la pince gauche
if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
+ XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
} else {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+ XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
}
break;
case 205://Sécuriser le palet gauche
@@ -257,6 +273,18 @@
XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
}
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;
default:
return 0;//L'action n'existe pas, il faut utiliser le CAN
@@ -280,6 +308,9 @@
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
@@ -289,12 +320,143 @@
}
/****************************************************************************************/
+/* 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();
}
/****************************************************************************************/
@@ -304,6 +466,8 @@
/****************************************************************************************/
int SelectStrategy(unsigned char id)
{
+
+
switch(id)
{
case 1:
@@ -321,10 +485,34 @@
case 5:
strcpy(cheminFileStart,"/local/strat5.txt");
return FileExists(cheminFileStart);
+ case 6:
+ strcpy(cheminFileStart,"/local/strat6.txt");
+ return FileExists(cheminFileStart);
+ case 10:
+ strcpy(cheminFileStart,"/local/grand_8.txt");
+ return FileExists(cheminFileStart);
default:
- strcpy(cheminFileStart,"/local/strat.txt");
+ 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)
+{
+
+}
+
#endif