CRAC Team
Strategie_13h30
Fork of
CRAC-Strat_2017_homologation_gros_rob
by 
CRAC Team
Diff: Robots/Strategie_small.cpp
- Revision:
- 15:c2fc239e85df
- Parent:
- 12:14729d584500
- Child:
- 16:7321fb3bb396
--- a/Robots/Strategie_small.cpp Fri Mar 31 16:20:26 2017 +0000
+++ b/Robots/Strategie_small.cpp Thu May 11 12:55:52 2017 +0000
@@ -36,243 +36,78 @@
/****************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
switch(id) {
- case 101://Descendre le bras pour pecher les poissons
+ 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
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- 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;
- } else {
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- 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;
- }
- break;
- case 102://Remonter le bras avec les poissons dessus
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
- }
- break;
- case 103://Decendre à 30°
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_RELACHER,
- AX12_SPEED_BRAS_BASE
- );
- 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_RELACHER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
- }
- break;
- case 104://ouvrir 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_OUVERT,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_RELACHEUR_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
- } else {
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_GAUCHE,
- AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_RELACHEUR_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
+
}
break;
- case 105://Rentrer le bras dans le robot, fermer le séparateur
+ case 111:// securiser un module avec une porte avant
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_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
- } else {
- AX12_setGoal(
- 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_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
+ if(speed == 1) speed = 0;
+ else speed = 1;
+ } else {
+
}
break;
- case 106://descendre l'attrape coquillages au niveau des rochets gauche si non inversé, droit si inversé
+ case 112:// ranger le porte avant dans le robot
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_ROCHET,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_ROCHET,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
+
}
break;
- case 107://descendre l'attrape coquillages au niveau de la table gauche si non inversé, droit si inversé
+ case 120:// poser une main tourneuse
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_OUVERT,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_OUVERT,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
- }
- break;
- case 108://Remonter l'attrape coquillages gauche si non inversé, droit si inversé
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_FERMER,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
- } else {
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_FERMER,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
+
}
break;
-
- case 200://ouvir la pince gauche
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
+ case 121:// relever une main tourneuse
+ if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
- }
- break;
- case 205://Sécuriser le palet gauche
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
- } else {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
- }
- break;
- case 202://fermer pince gauche
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
- } else {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+
}
break;
- case 201://ouvir la pince droite
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
+ case 122:// tourner un module
+ if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
+
}
break;
- case 204://Sécuriser le palet droit
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
- } else {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
- }
- break;
- case 203://fermer pince droite
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
- } else {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
- }
- break;
+
case 10://Désactiver le stop
isStopEnable = 0;
break;
@@ -302,12 +137,12 @@
/**
On enregistre les id des AX12 présent sur la carte
**/
- AX12_register(1,AX12_SERIAL1,0x0FF);
+ /*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);
+ AX12_register(17,AX12_SERIAL2,0x0FF);*/
//runRobotTest();
@@ -325,7 +160,7 @@
/****************************************************************************************/
void initRobotActionneur(void)
{
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+ /*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
AX12_setGoal(
@@ -358,7 +193,7 @@
AX12_ANGLE_PALET_GAUCHE_FERMER,
AX12_SPEED_PALET
);
- AX12_processChange();
+ AX12_processChange(); */
}
/****************************************************************************************/
@@ -367,7 +202,7 @@
/****************************************************************************************/
void runRobotTest(void)
{
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
+ /*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);
@@ -456,7 +291,7 @@
AX12_ANGLE_PALET_GAUCHE_FERMER,
AX12_SPEED_PALET
);
- AX12_processChange();
+ AX12_processChange();*/
}
/****************************************************************************************/