CRAC Team
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Robots/Strategie_big.cpp
- Revision:
- 15:c2fc239e85df
- Parent:
- 14:c8fc06c4887f
- Child:
- 16:7321fb3bb396
diff -r c8fc06c4887f -r c2fc239e85df Robots/Strategie_big.cpp
--- a/Robots/Strategie_big.cpp Fri Mar 31 16:20:26 2017 +0000
+++ b/Robots/Strategie_big.cpp Thu May 11 12:55:52 2017 +0000
@@ -1,3 +1,4 @@
+
#include "StrategieManager.h"
#ifdef ROBOT_BIG
#include "Config_big.h"
@@ -9,8 +10,8 @@
/* 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();
+ /* AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
+ AX12_processChange();*/
}
@@ -22,88 +23,138 @@
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();
+ case 100://preparation prise
+ 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;
+ }
+ msgTx.data[1]=speed;
+ can1.write(msgTx);
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();
+ case 101://stockage haut
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=2;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 102://Remonter le peigne
- AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP);
- SendRawId(0x123);
- AX12_processChange();
+ case 102://stockage bas
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=3;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 103://Descendre le peigne
- AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN);
- AX12_processChange();
+ case 103://deposer
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=4;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
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();
+ case 104://preparation depot bas
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=5;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 106://Remonter le support du cone arriere
- AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE);
- AX12_processChange();
+ case 105://preparation depot haut
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=6;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 107://Descendre le support du cone arriere
- AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE);
- AX12_processChange();
+ case 106://pousser module
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=7;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
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);
- AX12_processChange();
+ 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
- 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;*/
+ 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
- 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;
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
- case 114://Ouvrir les portes arrières
- AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 250);
- AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 50);
- AX12_processChange();
+ // action et le cote selectionné
+ msgTx.data[0]=12;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
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, 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();
+ /*AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
+ AX12_processChange();*/
msgTx.id=POMPE_PWM;
@@ -161,14 +212,42 @@
case 40: // demande au telemetre la position d'un objet
//SendRawId(TELEMETRE_RECHERCHE_OBJET);
- waitingAckID = ACKNOWLEDGE_TELEMETRE;
- waitingAckFrom = 0;
- modeTelemetre =1;
+
+ modeTelemetre = 1;
+
+ //angle = angle /10;
+
+ msgTx.id=TELEMETRE_OBJET;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=1;
+ // 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);
+ // y sur 2 octets
+ msgTx.data[2]=(unsigned char)angle;
+ msgTx.data[3]=(unsigned char)(angle>>8);
+ // theta signé sur 2 octets
+ //msgTx.data[4]=(unsigned char)theta;
+ //msgTx.data[5]=(unsigned char)(theta>>8);
+ msgTx.data[4]=0;
+ msgTx.data[5]=0;*/
+
+ can1.write(msgTx);
+
+ 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;
+ msgTx.id=ACTION_BIG_DEMARRAGE;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=1;
+ msgTx.data[0] = (unsigned char)speed;
+ can1.write(msgTx);
break;
default:
@@ -186,13 +265,13 @@
void initRobot(void)
{
//Enregistrement de tous les AX12 présent sur la carte
- AX12_register(5, AX12_SERIAL2);
+ /*AX12_register(5, AX12_SERIAL2);
AX12_register(18, AX12_SERIAL2);
AX12_register(13, AX12_SERIAL2);
AX12_register(1, AX12_SERIAL1);
AX12_register(11, AX12_SERIAL1);
AX12_register(8, AX12_SERIAL1);
- AX12_register(7, AX12_SERIAL2);
+ AX12_register(7, AX12_SERIAL2);*/
//AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
//AX12_processChange();
@@ -252,6 +331,7 @@
{
switch(id)
{
+ // strat de match
case 1:
strcpy(cheminFileStart,"/local/strat1.txt");
return FileExists(cheminFileStart);
@@ -283,14 +363,24 @@
strcpy(cheminFileStart,"/local/strat10.txt");
return FileExists(cheminFileStart);
- //
+ // strat de demo
case 0x10:
strcpy(cheminFileStart,"/local/moteur.txt");
return FileExists(cheminFileStart);
case 0x11:
- strcpy(cheminFileStart,"/local/moteur2.txt");
+#ifdef ROBOT_BIG
+ strcpy(cheminFileStart,"/local/bras.txt");
+#else
+ strcpy(cheminFileStart,"/local/porteAvant.txt");
+#endif
return FileExists(cheminFileStart);
-
+ case 0x12:
+#ifdef ROBOT_BIG
+ strcpy(cheminFileStart,"/local/balancier.txt");
+#else
+ strcpy(cheminFileStart,"/local/mainTourneuse.txt");
+#endif
+ return FileExists(cheminFileStart);
default:
strcpy(cheminFileStart,"/local/strat1.txt");
return 0;