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
diff -r 41e02746041d -r a1e37af4bbde Robots/Strategie_big.cpp --- 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