homologation gros robot
Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by
Diff: Robots/Strategie_small.cpp
- Revision:
- 11:ed13a480ddca
- Parent:
- 10:a788d9cf60f2
- Child:
- 12:14729d584500
diff -r a788d9cf60f2 -r ed13a480ddca Robots/Strategie_small.cpp --- 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;