![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Robots/Strategie_big.cpp
- Revision:
- 12:14729d584500
- Parent:
- 11:ed13a480ddca
- Child:
- 14:c8fc06c4887f
diff -r ed13a480ddca -r 14729d584500 Robots/Strategie_big.cpp --- a/Robots/Strategie_big.cpp Mon May 02 19:40:59 2016 +0000 +++ b/Robots/Strategie_big.cpp Mon May 09 09:10:17 2016 +0000 @@ -2,13 +2,14 @@ #ifdef ROBOT_BIG #include "Config_big.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 */ /****************************************************************************************/ void doFunnyAction(void) { - AX12_setGoal(AX12_ID_FUNNY_ACTION, 275); - AX12_setGoal(AX12_ID_FUNNY_ACTION, 180); + AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION); AX12_processChange(); } @@ -18,8 +19,46 @@ /* DESCRIPTION : Effectuer une action specifique */ /****************************************************************************************/ unsigned char doAction(unsigned char id, unsigned short speed, short angle) { + CANMessage msgTx=CANMessage(); switch(id) { + case 100://Ouvrir les portes avant + AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE); + AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE); + AX12_processChange(); + break; + case 101://Fermer les portes avant + AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER); + AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_FERMER); + AX12_processChange(); + break; + + case 102://Remonter le peigne + AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP); + AX12_processChange(); + break; + case 103://Descendre le peigne + AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN); + AX12_processChange(); + break; + + case 104://Monter le support ventouse haut + AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP); + AX12_processChange(); + break; + case 105://Descendre le support ventouse haut + AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_DOWN); + AX12_processChange(); + break; + case 106://Remonter le support du cone arriere + AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE); + AX12_processChange(); + break; + case 107://Descendre le support du cone arriere + AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE); + AX12_processChange(); + break; + case 110://Ouvrir la pince arrière haute AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 205); AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 95); @@ -29,17 +68,21 @@ AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 145); AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 155); AX12_processChange(); + /*waitingAckID = AX12_ID_PINCE_ARRIERE_HAUTE_DROITE; + waitingAckFrom = SERVO_AX12_DONE;*/ break; case 112://Ouvrir la pince arrière basse - AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 205); - AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 95); + AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 215); + AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 85); AX12_processChange(); break; case 113://Fermer la pince arrière basse AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 145); AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 155); AX12_processChange(); + /* waitingAckID = AX12_ID_PINCE_ARRIERE_BASSE_DROITE; + waitingAckFrom = SERVO_AX12_DONE;*/ break; case 114://Ouvrir les portes arrières @@ -48,10 +91,79 @@ AX12_processChange(); break; case 115://Fermer les portes arrière - AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145); - AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155); + //AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145); + //AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155); + AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 142); + AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 158); + AX12_processChange(); + //waitingAckID = AX12_ID_PORTE_ARRIERE_DROITE; + //waitingAckFrom = SERVO_AX12_DONE; + break; + + case 120://Activer les pompes + AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE); AX12_processChange(); + + + msgTx.id=POMPE_PWM; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.len=6; + + // x sur 2 octets + msgTx.data[0]=(unsigned char)POMPES_PWM; + msgTx.data[1]=(unsigned char)POMPES_PWM; + msgTx.data[2]=(unsigned char)POMPES_PWM; + msgTx.data[3]=(unsigned char)POMPES_PWM; + msgTx.data[4]=(unsigned char)POMPES_PWM; + msgTx.data[5]=(unsigned char)POMPES_PWM; + + can1.write(msgTx); break; + case 121://Désactiver les pompes + msgTx.id=POMPE_PWM; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.len=6; + + // x sur 2 octets + msgTx.data[0]=(unsigned char)0; + msgTx.data[1]=(unsigned char)0; + msgTx.data[2]=(unsigned char)0; + msgTx.data[3]=(unsigned char)0; + msgTx.data[4]=(unsigned char)0; + msgTx.data[5]=(unsigned char)0; + + can1.write(msgTx); + 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; + + case 22://Changer la vitesse du robot + SendSpeed(speed,(unsigned short)angle); + break; + + case 30://Action tempo + wait_ms(speed); + break; + + case 130://Lancer mouvement de sortie de la zone de départ + SendRawId(ACTION_BIG_DEMARRAGE); + waitingAckID = ACTION_BIG_DEMARRAGE; + waitingAckFrom = INSTRUCTION_END_MOTEUR; + break; + default: return 0;//L'action n'existe pas, il faut utiliser le CAN @@ -67,35 +179,61 @@ void initRobot(void) { //Enregistrement de tous les AX12 présent sur la carte - AX12_register(4, AX12_SERIAL1); - AX12_register(14, AX12_SERIAL1); - AX12_register(15, AX12_SERIAL1); AX12_register(5, AX12_SERIAL2); AX12_register(18, AX12_SERIAL2); AX12_register(13, AX12_SERIAL2); - AX12_register(1, AX12_SERIAL2); + AX12_register(1, AX12_SERIAL1); + AX12_register(11, AX12_SERIAL1); + AX12_register(8, AX12_SERIAL1); + AX12_register(7, AX12_SERIAL2); + //AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION); + //AX12_processChange(); //runRobotTest(); } /****************************************************************************************/ +/* FUNCTION NAME: initRobotActionneur */ +/* DESCRIPTION : Initialiser la position des actionneurs du robot */ +/****************************************************************************************/ +void initRobotActionneur(void) +{ + doAction(110,0,0);//Ouverture pince arrière haute + doAction(112,0,0);//Ouverture pince arrière basse + doAction(114,0,0);//Ouverture porte arrière + doAction(100,0,0);//Ouvrir les portes avant + doAction(102,0,0);//Remonter le peigne + doAction(106,0,0);//Remonter le support du cone arriere +} + +/****************************************************************************************/ /* FUNCTION NAME: runTest */ /* DESCRIPTION : tester l'ensemble des actionneurs du robot */ /****************************************************************************************/ void runRobotTest(void) { + int waitTime = 500; + //Test des AX12 dans l'ordre doAction(111,0,0);//Fermeture pince arrière haute - wait_ms(500); + wait_ms(waitTime); doAction(110,0,0);//Ouverture pince arrière haute - wait_ms(500); + wait_ms(waitTime); doAction(113,0,0);//Fermeture pince arrière basse - wait_ms(500); + wait_ms(waitTime); doAction(112,0,0);//Ouverture pince arrière basse - wait_ms(500); + wait_ms(waitTime); doAction(115,0,0);//Fermeture porte arrière - wait_ms(500); + wait_ms(waitTime); doAction(114,0,0);//Ouverture porte arrière + wait_ms(waitTime); + doAction(101,0,0);//Fermer les portes avant + wait_ms(waitTime); + doAction(100,0,0);//Ouvrir les portes avant + wait_ms(waitTime); + doAction(103,0,0);//Descendre le peigne + wait_ms(waitTime); + doAction(102,0,0);//Remonter le peigne } /****************************************************************************************/ @@ -116,10 +254,38 @@ 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"); + 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) +{ + doAction(110,0,0);//Ouverture pince arrière haute + doAction(112,0,0);//Ouverture pince arrière basse + doAction(114,0,0);//Ouverture porte arrière + doAction(100,0,0);//Ouvrir les portes avant + doAction(102,0,0);//Remonter le peigne +} + #endif