CRAC Team
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Robots/Strategie_big.cpp
- Revision:
- 30:a1e37af4bbde
- Parent:
- 29:41e02746041d
- Child:
- 36:6dd30780bd8e
--- a/Robots/Strategie_big.cpp Fri Apr 06 13:47:19 2018 +0000
+++ b/Robots/Strategie_big.cpp Fri Apr 20 09:16:13 2018 +0000
@@ -2,8 +2,6 @@
#ifdef ROBOT_BIG
-
-
unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
/****************************************************************************************/
@@ -27,494 +25,78 @@
/* FUNCTION NAME: doAction */
/* DESCRIPTION : Effectuer une action specifique */
/****************************************************************************************/
-unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
+unsigned char doAction(unsigned char id, unsigned short var1, short var2) {
CANMessage msgTx=CANMessage();
- int localData = 0, localData2;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ //affichage_debug(id);
+
switch(id) {
- case 100: // position initiale
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
-
- msgTx.data[0]=1;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 101: /// preparation prise
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
-
- msgTx.data[0]=2;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 102: // stockage haut
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
+ /////////////////////////////////////////////////////////100 à 108 : ACTIONS HERKULEX/////////////////////////////////////////////
+ /*case 100: //Monte l'immeuble selon le code couleur
+ msgTx.id=MONTER_IMMEUBLE;
- msgTx.data[0]=3;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 103: // stockage bas
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
-
- msgTx.data[0]=4;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 104: // pousser module
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
-
- msgTx.data[0]=7;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 105: // preparation prise
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
-
- msgTx.data[0]=6;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 106: //deposer module
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
-
- msgTx.data[0]=5;
- msgTx.data[1]=speed;
+ msgTx.len=3;
+ msgTx.data[0]=couleur1;
+ msgTx.data[1]=couleur2;
+ msgTx.data[2]=couleur3;
can1.write(msgTx);
- break;
-
- case 110: // repos bras avant
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- speed = BRAS_AVANT;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
- if (speed == BRAS_AVANT)localData = 0x10;
- else localData = 0x24;
-
- msgTx.data[0]=localData;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 111: // init bras module
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- speed = BRAS_AVANT;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
- if (speed == BRAS_AVANT)localData = 0x0A;
- else localData = 0x1E;
-
- msgTx.data[0]=localData;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 112: // bouger module avant la gouttière
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- speed = BRAS_AVANT;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
- if (speed == BRAS_AVANT)localData = 0x0B;
- else localData = 0x1F;
-
- msgTx.data[0]=localData;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 113: // deposer module
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- localData2 = BRAS_AVANT;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
- else localData2 = BRAS_AVANT;
- }
-
- if(localData2 == BRAS_AVANT){
- if(speed == INV){
- localData = 0x0E;
- }else{
- localData = 0x0C;
- }
- }else{
- if(speed == INV){
- localData = 0x22;
- }else{
- localData = 0x20;
- }
- }
-
- msgTx.data[0]=localData;
- msgTx.data[1]=localData2;
- can1.write(msgTx);
- break;
-
- case 114: // repositionner bras avant
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- speed = BRAS_AVANT;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
- if (speed == BRAS_AVANT)localData = 0x0F;
- else localData = 0x23;
-
- msgTx.data[0]=localData;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 115: // pompe avant ON
- msgTx.id=0x400;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- localData = BRAS_AVANT;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
- else localData = BRAS_AVANT;
- }
-
- msgTx.data[0]=1;
- msgTx.data[1]=localData;
-
- can1.write(msgTx);
- wait_ms(1);
- can1.write(msgTx);
- break;
+ break;*/
- case 116: // pompe avant OFF
- msgTx.id=0x400;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- localData = BRAS_AVANT;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
- else localData = BRAS_AVANT;
- }
-
- msgTx.data[0]=0;
- msgTx.data[1]=localData;
-
- can1.write(msgTx);
- wait_ms(1);
- can1.write(msgTx);
- break;
-
- case 120: // repos bras arrière
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- speed = BRAS_ARRIERE;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
- if (speed == BRAS_AVANT)localData = 0x10;
- else localData = 0x24;
-
- msgTx.data[0]=localData;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 121: // position d'init avant de prendre un module bras arriere
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- speed = BRAS_ARRIERE;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
- if (speed == BRAS_AVANT)localData = 0x0A;
- else localData = 0x1E;
-
- msgTx.data[0]=localData;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 122: // bouger le bras arriere avant la gouttiere
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- speed = BRAS_ARRIERE;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
- if (speed == BRAS_AVANT)localData = 0x0B;
- else localData = 0x1F;
-
- msgTx.data[0]=localData;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 123: // deposer module
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- localData2 = BRAS_ARRIERE;
-
- if (InversStrat){ // si on est inversé, on echange les bras
- if (localData2 == BRAS_AVANT) localData2 = BRAS_ARRIERE;
- else localData2 = BRAS_AVANT;
- }
-
- if(localData2 == BRAS_AVANT){
- if(speed == INV){
- localData = 0x0E;
- }else{
- localData = 0x0C;
- }
- }else{
- if(speed == INV){
- localData = 0x22;
- }else{
- localData = 0x20;
- }
- }
-
- msgTx.data[0]=localData;
- msgTx.data[1]=localData2;
- can1.write(msgTx);
- break;
-
- case 124: // remettre le bras arriere en posiion initiale
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- speed = BRAS_ARRIERE;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
- else speed = BRAS_AVANT;
- }
- if (speed == BRAS_AVANT)localData = 0x0F;
- else localData = 0x23;
+ case 101: //baisser l'attrape bloc arrière
+ SendRawId(BAISSER_ATTRAPE_BLOC);
+ wait_us(50);
+ break;
+ case 102: //Relever l'attrape bloc arrière
+ SendRawId(RELEVER_ATTRAPE_BLOC);
+ break;
+ case 103: //
+ SendRawId(BAISSER_ATTRAPE_BLOC_CENTRE_G);
+ break;
+ case 104:
+ SendRawId(RELEVER_ATTRAPE_BLOC_CENTRE_G);
+ break;
+ case 105:
+ SendRawId(BAISSER_ATTRAPE_BLOC_CENTRE_D);
+ break;
+ case 106:
+ SendRawId(RELEVER_ATTRAPE_BLOC_CENTRE_D);
+ break;
+ case 107:
+ SendRawId(RELEVER_ATTRAPE_BLOC_EXT_D);
+ break;
+ case 108:
+ SendRawId(BAISSER_ATTRAPE_BLOC_EXT_D);
+ break;
+ case 109:
+ SendRawId(BAISSER_ATTRAPE_BLOC_EXT_G);
+ break;
+ case 110:
+ SendRawId(RELEVER_ATTRAPE_BLOC_EXT_G);
+ break;
+ case 111:
+ SendRawId(BRAS_VENTOUSE_1);
+ break;
+ case 112:
+ SendRawId(BRAS_VENTOUSE_2);
+ break;
+ case 113:
+ SendRawId(LIRE_PANNEAU);
+ break;
+ case 114: //allumer panneau
+ SendRawId(ALLUMER_PANNEAU_UP);
+ break;
+ case 115: //range le bras qui allume le panneau
+ SendRawId(ALLUMER_PANNEAU_DOWN);
+ break;
+ case 116: //lève le bras pour pousser l'abeille
+ SendRawId(BRAS_ABEILLE_UP);
+ break;
+ case 117: //abaisse le bras qui pousse l'abeille
+ SendRawId(BRAS_ABEILLE_DOWN);
+ break;
+ ////////////////////////////////////////////////////////130 à 134 GESTIONS BALLES/////////////////////////////////////////
- msgTx.data[0]=localData;
- msgTx.data[1]=speed;
- can1.write(msgTx);
- break;
-
- case 125: // pompe pwm arriere ON
- msgTx.id=0x400;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- localData = BRAS_ARRIERE;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
- else localData = BRAS_AVANT;
- }
-
- msgTx.data[0]=1;
- msgTx.data[1]=localData;
-
- can1.write(msgTx);
- wait_ms(1);
- can1.write(msgTx);
- break;
-
- case 126: // pompe arriere pwm OFF
- msgTx.id=0x400;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- localData = BRAS_ARRIERE;
- if (InversStrat){ // si on est inversé, on echange les bras
- if (localData == BRAS_AVANT) localData = BRAS_ARRIERE;
- else localData = BRAS_AVANT;
- }
-
- msgTx.data[0]=0;
- msgTx.data[1]=localData;
-
- can1.write(msgTx);
- wait_ms(1);
- can1.write(msgTx);
- break;
-
- case 130: // baiser bras turbine
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- msgTx.data[0]=0x14;
- msgTx.data[1]=BRAS_AVANT;
- can1.write(msgTx);
- break;
-
- case 131: // lever bras turbine
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- msgTx.data[0]=0x15;
- msgTx.data[1]=BRAS_AVANT;
- can1.write(msgTx);
- break;
-
- case 132: // allumer turbine
- msgTx.id=0x403;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- msgTx.data[0]=1;
- msgTx.data[1]=BRAS_AVANT;
- can1.write(msgTx);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
-
- case 133: // stop turbine
- msgTx.id=0x403;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
- msgTx.data[0]=0;
- msgTx.data[1]=BRAS_AVANT;
- can1.write(msgTx);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
-
- case 140:// lanceur ON
- msgTx.id=0x402;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- msgTx.data[0]=1;
- msgTx.data[1]=0x02;
- can1.write(msgTx);
- break;
-
- case 141: // lanceur OFF
- msgTx.id=0x402;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- msgTx.data[0]=0;
- msgTx.data[1]=0x02;
- can1.write(msgTx);
- break;
-
- case 150:
- msgTx.id=0x404;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- msgTx.data[0]=1;
- msgTx.data[1]=BRAS_AVANT;
-
- can1.write(msgTx);
- break;
-
- case 151:
- msgTx.id=0x404;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- msgTx.data[0]=0;
- msgTx.data[1]=BRAS_AVANT;
-
- can1.write(msgTx);
- break;
-
case 10://Désactiver le stop
isStopEnable = 0;
break;
@@ -529,14 +111,14 @@
break;
case 22://Changer la vitesse du robot
- SendSpeed(speed,(unsigned short)angle);
+ SendSpeed(var1,(unsigned short)var2);
waitingAckFrom = 0;
waitingAckID = 0;
wait(0.2);
break;
case 30://Action tempo
- wait_ms(speed);
+ wait_ms(var1);
break;
/*case 40: // demande au telemetre la position d'un objet