CRAC Team
v2019
Dependencies: CRAC-Strat_2019 SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Dependents: Codeprincipal_2019 CRAC-Strat_2019
Diff: Robots/Strategie_big.cpp
- Revision:
- 18:cc5fec34ed9c
- Parent:
- 16:7321fb3bb396
- Child:
- 19:b4b91258c275
diff -r d1594579eec6 -r cc5fec34ed9c Robots/Strategie_big.cpp
--- a/Robots/Strategie_big.cpp Fri May 19 17:14:07 2017 +0000
+++ b/Robots/Strategie_big.cpp Mon May 22 15:01:49 2017 +0000
@@ -10,9 +10,16 @@
/* DESCRIPTION : Permet de faire la funny action en fin de partie */
/****************************************************************************************/
void doFunnyAction(void) {
- /* AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
- AX12_processChange();*/
-
+ //envoie de la funny action
+ // 0x007, 01, 01
+ CANMessage msgTx=CANMessage();
+ msgTx.id=GLOBAL_FUNNY_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+ msgTx.data[0]=0x01;
+ msgTx.data[1]=0x01;
+ can1.write(msgTx);
}
/****************************************************************************************/
@@ -21,171 +28,485 @@
/****************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
CANMessage msgTx=CANMessage();
+ int localData = 0, localData2;
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 100://preparation prise
+ 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;
-
- // action et le cote selectionné
- msgTx.data[0]=1;
+
if (InversStrat){ // si on est inversé, on echange les bras
- if (speed == BRAS_GAUCHE) speed = BRAS_DROIT;
- else speed = BRAS_GAUCHE;
+ if (speed == BRAS_AVANT) speed = BRAS_ARRIERE;
+ else speed = BRAS_AVANT;
}
+
+ msgTx.data[0]=3;
msgTx.data[1]=speed;
can1.write(msgTx);
break;
- case 101://stockage haut
+
+ 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;
-
- // action et le cote selectionné
- msgTx.data[0]=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 102://stockage bas
+ 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;
+ 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;
-
- // action et le cote selectionné
- msgTx.data[0]=3;
+ 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 103://deposer
+
+ case 112: // bouger module avant la gouttière
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
-
- // action et le cote selectionné
- msgTx.data[0]=4;
+ 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 104://preparation depot bas
+ case 113: // deposer module
msgTx.id=SERVO_AX12_ACTION;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=2;
-
- // action et le cote selectionné
- msgTx.data[0]=5;
+ 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]=speed;
-
can1.write(msgTx);
break;
- case 105://preparation depot haut
+
+ 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;
- // action et le cote selectionné
- msgTx.data[0]=6;
- msgTx.data[1]=speed;
+ 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;
+ 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 106://pousser module
+
+ 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 = 0xA0;
+ 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;
-
- // action et le cote selectionné
- msgTx.data[0]=7;
+ 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]=speed;
-
can1.write(msgTx);
break;
- case 110://Ouvrir la pince arrière haute
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=2;
-
- // action et le cote selectionné
- msgTx.data[0]=10;
- msgTx.data[1]=speed;
-
- can1.write(msgTx);
- break;
- case 111://Fermer la pince arrière haute
- msgTx.id=SERVO_AX12_ACTION;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.len=3;
-
- // action et le cote selectionné
- msgTx.data[0]=11;
- msgTx.data[1]=speed;
- msgTx.data[2]=angle;
-
- can1.write(msgTx);
- break;
-
- case 112://Ouvrir la pince arrière basse
+ 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;
+
+ msgTx.data[0]=localData;
+ msgTx.data[1]=speed;
+ can1.write(msgTx);
+ break;
- // action et le cote selectionné
- msgTx.data[0]=12;
- msgTx.data[1]=speed;
+ 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);
+ 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);
+ 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 120://Activer les pompes
- /*AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
- AX12_processChange();*/
-
-
- msgTx.id=POMPE_PWM;
+ case 150:
+ msgTx.id=0x404;
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;
-
+ msgTx.len=2;
+
+ msgTx.data[0]=1;
+ msgTx.data[1]=BRAS_AVANT;
+
can1.write(msgTx);
break;
- case 121://Désactiver les pompes
- msgTx.id=POMPE_PWM;
+
+ case 151:
+ msgTx.id=0x404;
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;
-
+ msgTx.len=2;
+
+ msgTx.data[0]=0;
+ msgTx.data[1]=BRAS_AVANT;
+
can1.write(msgTx);
break;
@@ -210,7 +531,7 @@
wait_ms(speed);
break;
- case 40: // demande au telemetre la position d'un objet
+ /*case 40: // demande au telemetre la position d'un objet
//SendRawId(TELEMETRE_RECHERCHE_OBJET);
modeTelemetre = 1;
@@ -224,7 +545,7 @@
// indice du module sur le terrain
msgTx.data[0] = (unsigned char)speed;
- /*
+
// x sur 2 octets
msgTx.data[0]=(unsigned char)speed;
msgTx.data[1]=(unsigned char)(speed>>8);
@@ -235,20 +556,20 @@
//msgTx.data[4]=(unsigned char)theta;
//msgTx.data[5]=(unsigned char)(theta>>8);
msgTx.data[4]=0;
- msgTx.data[5]=0;*/
+ msgTx.data[5]=0;
can1.write(msgTx);
- break;
+ break;*/
- case 130://Lancer mouvement de sortie de la zone de départ
+ /* case 130://Lancer mouvement de sortie de la zone de départ
msgTx.id=ACTION_BIG_DEMARRAGE;
msgTx.format=CANStandard;
msgTx.type=CANData;
msgTx.len=1;
msgTx.data[0] = (unsigned char)speed;
can1.write(msgTx);
- break;
+ break;*/
default:
return 0;//L'action n'existe pas, il faut utiliser le CAN
@@ -285,12 +606,14 @@
/****************************************************************************************/
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
+ wait(2);
+ doAction(101,1,0);// preparation prise pince arriere
+ doAction(101,0,0);// preparation prise pince avant
+ doAction(130,0,0);// baisser aspitr
+ wait(2);
+ doAction(100,1,0);// preparation prise pince arriere
+ doAction(100,0,0);// preparation prise pince avant
+ doAction(131,0,0);// baisser aspitr
}
/****************************************************************************************/
@@ -299,6 +622,7 @@
/****************************************************************************************/
void runRobotTest(void)
{
+ /*
int waitTime = 500;
//Test des AX12 dans l'ordre
@@ -320,7 +644,7 @@
wait_ms(waitTime);
doAction(103,0,0);//Descendre le peigne
wait_ms(waitTime);
- doAction(102,0,0);//Remonter le peigne
+ doAction(102,0,0);//Remonter le peigne*/
}
/****************************************************************************************/