Strategie_13h30
Fork of CRAC-Strat_2017_homologation_gros_rob by
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();*/ } /****************************************************************************************/