CRAC Team
code petit robot pour homologation
Fork of
CRAC-Strat_2017_V2
by 
CRAC Team
Diff: Robots/Strategie_small.cpp
- Revision:
- 11:ed13a480ddca
- Parent:
- 10:a788d9cf60f2
- Child:
- 12:14729d584500
--- a/Robots/Strategie_small.cpp Fri Apr 29 09:14:27 2016 +0000
+++ b/Robots/Strategie_small.cpp Mon May 02 19:40:59 2016 +0000
@@ -35,77 +35,234 @@
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
switch(id) {
case 101://Descendre le bras pour pecher les poissons
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté gauche
- AX12_setGoal(AX12_ID_BRAS_BASE_INV,200,0x0FF);
+ 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_processChange();
+ waitingAckID = AX12_ID_BRAS_BASE_DROIT;
+ waitingAckFrom = SERVO_AX12_DONE;
} else {
- AX12_setGoal(AX12_ID_BRAS_BASE,140,0x0FF);
+ AX12_setGoal(
+ AX12_ID_BRAS_BASE_GAUCHE,
+ AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT,
+ AX12_SPEED_BRAS_BASE
+ );
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é gauche
- AX12_setGoal(AX12_ID_BRAS_BASE_INV,220,0x0FF);
+ 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;
} else {
- AX12_setGoal(AX12_ID_BRAS_BASE,100,0x0FF);
+ 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é gauche
- AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,90);//Ouverture du bras
+ 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_RELACHEUR,250);//Ouverture du bras
+ 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é gauche
- AX12_setGoal(AX12_ID_BRAS_BASE_INV,278,0x0FF);
- AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
+ 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_BASE,55,0x0FF);
- AX12_setGoal(AX12_ID_BRAS_RELACHEUR,200);//fermer le bras
+ 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
- //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
- //AX12_processChange();
+ 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_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_SPEED_BRAS_RELACHEUR
+ );
+ 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 106://descendre l'atrape coquillages gauche si non inversé, droit si inversé
- //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras
- //AX12_processChange();
+ case 106://descendre l'attrape coquillages au niveau des rochets 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_ROCHET,
+ 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_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é
+ 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;
+ } 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
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, 350);
+ 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 205://Sécuriser le palet gauche
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, 400);
+ 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
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, 650);
+ 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
- XL320_setGoal(XL320_ID_PINCE_DROITE, 400);
+ if(InversStrat == 1) {
+ XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
+ } else {
+ XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
+ }
break;
case 204://Sécuriser le palet droit
- XL320_setGoal(XL320_ID_PINCE_DROITE, 300);
+ 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
- XL320_setGoal(XL320_ID_PINCE_DROITE, 70);
+ 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;
default:
return 0;//L'action n'existe pas, il faut utiliser le CAN
}
- //return 1;//L'action est spécifique.
+ return 1;//L'action est spécifique.
}
@@ -158,6 +315,12 @@
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);
default:
strcpy(cheminFileStart,"/local/strat.txt");
return 0;