Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Robots/Strategie_small.cpp
- Revision:
- 10:a788d9cf60f2
- Parent:
- 9:d0042422d95a
- Child:
- 11:ed13a480ddca
--- a/Robots/Strategie_small.cpp Thu Apr 28 08:11:36 2016 +0000 +++ b/Robots/Strategie_small.cpp Fri Apr 29 09:14:27 2016 +0000 @@ -10,6 +10,23 @@ } +//L'angle est entre 0 et 1023 +void XL320_setGoal(unsigned char id, unsigned short angle); + +void XL320_setGoal(unsigned char id, unsigned short angle) +{ + CANMessage msgTx=CANMessage(); + msgTx.id=SERVO_XL320; + msgTx.len=3; + msgTx.format=CANStandard; + msgTx.type=CANData; + msgTx.data[0]=(unsigned char)id; + // from sur 2 octets + msgTx.data[1]=(unsigned char)angle; + msgTx.data[2]=(unsigned char)(angle>>8); + + can1.write(msgTx); +} /****************************************************************************************/ /* FUNCTION NAME: doAction */ @@ -17,7 +34,7 @@ /****************************************************************************************/ unsigned char doAction(unsigned char id, unsigned short speed, short angle) { switch(id) { - /*case 101://Descendre le bras pour les poissons + 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); AX12_processChange(); @@ -26,7 +43,7 @@ AX12_processChange(); } break; - case 102://Remonter bras moiter + 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); AX12_processChange(); @@ -35,7 +52,7 @@ AX12_processChange(); } break; - case 103://Lacher les poissons + 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 AX12_processChange(); @@ -44,7 +61,7 @@ AX12_processChange(); } break; - case 104://Rentrer le bras + 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 @@ -55,18 +72,37 @@ AX12_processChange(); } break; - case 105: + 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(); + 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(); break; - case 106: - //AX12_setGoal(AX12_ID_BRAS_RELACHEUR_INV,160);//fermer le bras - //AX12_processChange(); - break;*/ + + case 200://ouvir la pince gauche + XL320_setGoal(XL320_ID_PINCE_GAUCHE, 350); + break; + case 205://Sécuriser le palet gauche + XL320_setGoal(XL320_ID_PINCE_GAUCHE, 400); + break; + case 202://fermer pince gauche + XL320_setGoal(XL320_ID_PINCE_GAUCHE, 650); + break; + + case 201://ouvir la pince droite + XL320_setGoal(XL320_ID_PINCE_DROITE, 400); + break; + case 204://Sécuriser le palet droit + XL320_setGoal(XL320_ID_PINCE_DROITE, 300); + break; + case 203://fermer pince droite + XL320_setGoal(XL320_ID_PINCE_DROITE, 70); + break; + default: - waitingAckFrom = 0; - waitingAckID = 0; - return 1;//L'action n'existe pas, il faut utiliser le CAN + return 0;//L'action n'existe pas, il faut utiliser le CAN } //return 1;//L'action est spécifique.