CRAC Team
/
CRAC-Strat_2017_fin_premier_match
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
CRAC-Strat_2017_homologation_petit_rob
by 
CRAC Team
Revision 18:cc5fec34ed9c, committed 2017-05-22
- Comitter:
- ClementBreteau
- Date:
- Mon May 22 15:01:49 2017 +0000
- Parent:
- 17:d1594579eec6
- Child:
- 19:b4b91258c275
- Commit message:
- v2
Changed in this revision
--- a/Globals/constantes.h Fri May 19 17:14:07 2017 +0000
+++ b/Globals/constantes.h Mon May 22 15:01:49 2017 +0000
@@ -30,9 +30,9 @@
//#define POSITION_DEBUT_Y 900
//#define POSITION_DEBUT_T 180
- #define POSITION_DEBUT_X 2000-1830
- #define POSITION_DEBUT_Y 3000-900
- #define POSITION_DEBUT_T 0
+ #define POSITION_DEBUT_X 200
+ #define POSITION_DEBUT_Y 880
+ #define POSITION_DEBUT_T 385
//#define POSITION_DEBUT_X 0
//#define POSITION_DEBUT_Y 0
@@ -43,9 +43,9 @@
#else
#define NOMBRE_CARTES 1 //Le nombre de carte présente sur le petit robot
- #define POSITION_DEBUT_X 0
- #define POSITION_DEBUT_Y 0
- #define POSITION_DEBUT_T 0
+ #define POSITION_DEBUT_X 200
+ #define POSITION_DEBUT_Y 150
+ #define POSITION_DEBUT_T 900
#define BALISE_TIMEOUT 2000
--- a/Globals/global.h Fri May 19 17:14:07 2017 +0000 +++ b/Globals/global.h Mon May 22 15:01:49 2017 +0000 @@ -23,6 +23,8 @@ extern unsigned char nb_instructions; //Le nombre d'instruction dans le fichier de strategie extern unsigned char actual_instruction;//La ligne de l'instruction en cours d'execution +extern unsigned short telemetreDistance; + extern unsigned char InversStrat;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y) extern unsigned short waitingAckID;//L'id du ack attendu
--- a/Globals/ident_crac.h Fri May 19 17:14:07 2017 +0000 +++ b/Globals/ident_crac.h Mon May 22 15:01:49 2017 +0000 @@ -61,8 +61,6 @@ #define CHECK_OK_TELEMETRE 0x066 // Check telemetre - - #define ALIVE_BALISE 0x070 // Alive balise #define ALIVE_MOTEUR 0x071 // Alive moteur #define ALIVE_IHM 0x072 // Alive écran tactile
--- a/Robots/Config_big.h Fri May 19 17:14:07 2017 +0000 +++ b/Robots/Config_big.h Mon May 22 15:01:49 2017 +0000 @@ -3,8 +3,16 @@ /** Fichier de configuration des action specifique au gros robot **/ -#define BRAS_GAUCHE 1 -#define BRAS_DROIT 2 +#define BRAS_AVANT 1 +#define BRAS_ARRIERE 2 + +#define INV 1 + +#define AX12_MODULE_AV 0x0C +#define AX12_MODULE_AV_INV 0x0E + +#define AX12_MODULE_AR 0x20Z +#define AX12_MODULE_AR_INV 0x22 #define AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE 18
--- 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*/
}
/****************************************************************************************/
--- a/Robots/Strategie_small.cpp Fri May 19 17:14:07 2017 +0000
+++ b/Robots/Strategie_small.cpp Mon May 22 15:01:49 2017 +0000
@@ -3,6 +3,8 @@
#include "Config_small.h"
unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
+unsigned short telemetreDistance;
+
/****************************************************************************************/
/* FUNCTION NAME: doFunnyAction */
@@ -30,55 +32,74 @@
can1.write(msgTx);
}
+
/****************************************************************************************/
/* FUNCTION NAME: doAction */
/* DESCRIPTION : Effectuer une action specifique */
/****************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
+ int retour = 1;
switch(id) {
case 100: // preparation prise bras central
- Preparation_prise();
+ AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_PRISE);
break;
case 101:// stockage haut bras central
- Stockage_haut();
+ AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_HAUT);
break;
case 102:// stockage bas bras central
- Stockage_bas();
+ AX12_automate(AX12_PINCE_CENTRALE_STOCKAGE_BAS);
break;
case 103:// ouvrir la main du bras cental
- Deposer();
+ AX12_automate(AX12_PINCE_CENTRALE_DEPOSER);
break;
case 104:// preparation de depot du module bas du bras central
- Preparation_depot_bas();
+ AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
break;
case 105:// preparation de depot du module haut du bras central
- Preparation_depot_haut();
+ AX12_automate(AX12_PINCE_CENTRALE_DEPOT_HAUT);
+ AX12_automate(AX12_PINCE_CENTRALE_PREPARATION_DEPOT);
break;
- case 106:// positionner le bras afin de pousser un module debout
- Pousser_module();
+ case 106:// on rentre la pince dans le robot
+ AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
break;
+ case 107: // on ferme la pince du robot
+ AX12_automate(AX12_PINCE_CENTRALE_PRISE_MODULE);
+ break;
+
case 110:// ouvrir une porte avant
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
if(speed == 1) speed = 0;
else speed = 1;
- } else {
-
+ }
+
+ if (speed == 1){
+ AX12_automate(AX12_DROIT_CROC_OUVERT);
+ }else{
+ AX12_automate(AX12_GAUCHE_CROC_OUVERT);
}
break;
case 111:// securiser un module avec une porte avant
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
if(speed == 1) speed = 0;
else speed = 1;
- } else {
-
+ }
+
+ if (speed == 1){
+ AX12_automate(AX12_DROIT_CROC_FERME);
+ }else{
+ AX12_automate(AX12_GAUCHE_CROC_FERME);
}
break;
case 112:// ranger le porte avant dans le robot
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
if(speed == 1) speed = 0;
else speed = 1;
- } else {
-
+ }
+
+ if (speed == 1){
+ AX12_automate(AX12_DROIT_CROC_INITIALE);
+ }else{
+ AX12_automate(AX12_GAUCHE_CROC_INITIALE);
}
break;
case 120:// poser une main tourneuse
@@ -90,11 +111,10 @@
}
if (speed == 1){
- //Preparation_module_droit();
+ AX12_automate(AX12_TOURNANTE_DROIT_PREPARATION);
}else{
- //Preparation_module_gauche();
+ AX12_automate(AX12_TOURNANTE_GAUCHE_PREPARATION);
}
- Preparation_module_gauche();
break;
case 121:// relever une main tourneuse
@@ -106,29 +126,40 @@
}
if (speed == 1){
- //RangerBrasDroit();
+ AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
}else{
- RangerBrasGauche();
+ AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);
}
break;
case 122:// tourner un module
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+ if(InversStrat == 1) {//Si c'est inversé
if(speed == 2) speed = 1;
else speed = 2;
if(angle == 1) angle = 0;
else angle = 1;
}
- /*if (speed == 1){
- //Tourner_module_droite();
+ if (speed == 1){
+ AX12_automate(AX12_TOURNANTE_DROIT_MODULE);
+ Tourner_module_droit();
}else{
- Tourner_module_gauche();
- }*/
- Tourner_module_gauche();
-
-
+ AX12_automate(AX12_TOURNANTE_GAUCHE_MODULE);
+ Tourner_module_gauche();
+ }
+ break;
+
+ case 200 :
+ telemetreDistance = dataTelemetre();
+ wait_ms(1);
+ telemetreDistance = dataTelemetre();
+ telemetreDistance = telemetreDistance - 170;
+ break;
+
+ case 201 :
+ SendRawId(0x96);
+ retour = 2;
break;
case 10://Désactiver le stop
@@ -146,6 +177,7 @@
case 22://Changer la vitesse du robot
SendSpeed(speed,(unsigned short)angle);
+ wait_us(200);
break;
case 30://Action tempo
@@ -153,10 +185,10 @@
break;
default:
- return 0;//L'action n'existe pas, il faut utiliser le CAN
+ retour = 0;//L'action n'existe pas, il faut utiliser le CAN
}
- return 1;//L'action est spécifique.
+ return retour;//L'action est spécifique.
}
@@ -184,9 +216,8 @@
AX12_setGoal(AX12_ID_BRAS_RELACHEUR,160);//fermer le bras
AX12_processChange();*/
- initAX12();
- moteurGauchePWM(0);
- moteurDroitPWM(0);
+ initialisation_AX12();
+
}
/****************************************************************************************/
@@ -195,40 +226,13 @@
/****************************************************************************************/
void initRobotActionneur(void)
{
- /*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
-
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_DROIT,
- AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_GAUCHE,
- AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_FERMER,
- AX12_SPEED_PALET
- );
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_FERMER,
- AX12_SPEED_PALET
- );
- AX12_processChange(); */
+ moteurGauchePWM(0);
+ moteurDroitPWM(0);
+ AX12_automate(AX12_PINCE_CENTRALE_POSITION_INITIALE);
+ AX12_automate(AX12_GAUCHE_CROC_INITIALE);
+ AX12_automate(AX12_DROIT_CROC_INITIALE);
+ AX12_automate(AX12_TOURNANTE_GAUCHE_POSITION_INITIALE);
+ AX12_automate(AX12_TOURNANTE_DROIT_POSITION_INITIALE);
}
/****************************************************************************************/
@@ -237,96 +241,7 @@
/****************************************************************************************/
void runRobotTest(void)
{
- /*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
-
- wait_ms(200);
-
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
-
- wait_ms(200);
-
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
-
- wait_ms(500);
-
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_DROIT,
- AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_GAUCHE,
- AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
-
- wait_ms(600);
-
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_DROIT,
- AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_GAUCHE,
- AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
-
- wait_ms(500);
-
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
-
- wait_ms(600);
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_ROCHET,
- AX12_SPEED_PALET
- );
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_ROCHET,
- AX12_SPEED_PALET
- );
- AX12_processChange();
-
- wait_ms(500);
-
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_FERMER,
- AX12_SPEED_PALET
- );
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_FERMER,
- AX12_SPEED_PALET
- );
- AX12_processChange();*/
+
}
/****************************************************************************************/
--- a/Strategie/Strategie.cpp Fri May 19 17:14:07 2017 +0000
+++ b/Strategie/Strategie.cpp Mon May 22 15:01:49 2017 +0000
@@ -14,7 +14,7 @@
unsigned short waitingAckID = 0;//L'id du ack attendu
unsigned short waitingAckFrom = 0;//La provenance du ack attendu
char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre
-
+//unsigned short telemetreDistance = 0;
signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
signed short x_robot,y_robot,theta_robot;//La position du robot
@@ -36,7 +36,7 @@
unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
-InterruptIn jack(p24); // entrée analogique en interruption pour le jack
+InterruptIn jack(p25); // entrée analogique en interruption pour le jack
#endif
unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0;
@@ -242,9 +242,10 @@
localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
}
SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
-#endif /*
+#endif
+#ifdef ROBOT_SMALL
SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
-#endif*/
+#endif
break;
case ETAT_GAME_WAIT_FOR_JACK:
//On attend le jack
@@ -303,6 +304,7 @@
{
localData1 = -localData1;//Inversion de la direction
}
+
BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
break;
case MV_LINE://Ligne droite
@@ -390,20 +392,36 @@
GoStraight(localData2, localData5, localData3, 0);
break;
case ACTION:
-
- waitingAckID = SERVO_AX12_ACTION;
+ int tempo = 0;
+ waitingAckID= SERVO_AX12_ACTION;
waitingAckFrom = ACKNOWLEDGE_AX12;
- if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
+ tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
+ if(tempo == 1){
//L'action est spécifique
if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
- wait_us(200);
+
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
} else {
gameEtat = ETAT_GAME_WAIT_ACK;
}
+ #ifdef ROBOT_SMALL
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ #endif
return;
- } else {
+ #ifdef ROBOT_SMALL
+ } else if (tempo == 2) {
+ // on est dans le cas de l'avance selon le telemetre
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+
+ localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
+ GoStraight(telemetreDistance, 0, 0, 0);
+ // on reset la distance du telemetre à 0
+ telemetreDistance = 5000;
+ #endif
+ }else{
//C'est un AX12 qu'il faut bouger
//AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
//AX12_enchainement++;
@@ -479,8 +497,15 @@
case ACTION:
if (modeTelemetre == 0){
- waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;
- waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
+ if (telemetreDistance == 0){
+ waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;
+ waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
+ }else if(telemetreDistance == 5000){
+ // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = INSTRUCTION_END_MOTEUR;
+ telemetreDistance = 0;
+ }
}else{ // si on attend la reponse du telemetre
//modeTelemetre = 1;
waitingAckID = OBJET_SUR_TABLE;
@@ -601,6 +626,7 @@
break;
case ETAT_END_LOOP:
//Rien, on tourne en rond
+
break;
default:
@@ -651,6 +677,7 @@
case INSTRUCTION_END_MOTEUR:
case INSTRUCTION_END_IHM:
case INSTRUCTION_END_AX12:
+
if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
waitingAckFrom = 0;
waitingAckID = 0;
@@ -742,7 +769,7 @@
case BALISE_STOP:
SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
- if (instruction[actual_instruction].order == MV_TURN){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
+ //if (instruction[actual_instruction].order != MV_TURN){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
if(needToStop() != 0 && ingnorBaliseOnce ==0) {
if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)
{
@@ -755,7 +782,7 @@
}
}
}
- }
+ //}
ingnorBaliseOnce = 0;
break;
--- a/main.cpp Fri May 19 17:14:07 2017 +0000
+++ b/main.cpp Mon May 22 15:01:49 2017 +0000
@@ -32,14 +32,17 @@
can1.frequency(1000000); // fréquence de travail 1Mbit/s
can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
+
wait_ms(5000);
#ifdef ROBOT_BIG
tactile_printf("Initialisation gros robot");
#else
tactile_printf("Initialisation petit robot");
#endif
+ led1 = 1;
initRobot();//Initialisation du robot
-
+ initRobotActionneur();
+ led1 = 0;
wait_ms(2000);//Attente pour que toutes les cartes se lancent et surtout le CANBlue
/**
@@ -47,7 +50,7 @@
**/
//strcpy(cheminFileStart,"/local/test.txt");//On ouvre le fichier test.txt
//loadAllInstruction();//Mise en cache de toute les instructions
-
+
while(true) {
automate_process();//Boucle dans l'automate principal
canProcessRx();//Traitement des trames CAN en attente
--- a/peripheriques/actionneurs.cpp Fri May 19 17:14:07 2017 +0000
+++ b/peripheriques/actionneurs.cpp Mon May 22 15:01:49 2017 +0000
@@ -1,434 +1,64 @@
#include "peripheriques.h"
-/* contient les fonctiones qui servent à utiliser les AX12 et les moteurs sur le petit robot*/
+/* contient les fonctions qui servent à utiliser les AX12 et les moteurs sur le petit robot*/
+/*
+DigitalIn IO1(p23);
+DigitalIn IO2(p24);
+DigitalIn IO3(p25);
+DigitalIn IO4(p26);
+AnalogIn A_in1(p15);
+AnalogIn A_in2(p16);
+AnalogIn A_in3(p17);
+AnalogIn A_in4(p18);
+AnalogIn A_in5(p19);
+AnalogIn A_in6(p20);
+
+PwmOut IRL_1(p21);
+PwmOut IRL_2(p22);
+*/
PwmOut motGauche(p21);
PwmOut motDroit(p22);
-
-// AX12 partie centrale du petit robot
-AX12 *partieBasseCentre, *partiePoignetCentre, *partieMainGaucheCentre, *partieMainDroiteCentre;
-AX12 *partieDoigtCentre;
-AX12 *multipleCentre;
-// AX12 partie gauche du robot
-AX12 *partieBasseGauche, *partieMainGauche;
-AX12 *partiePorteGauche;
-AX12 *multipleGauche;
-
-// AX12 partie droite du robot
-AX12 *partieBasseDroite, *partieMainDroite;
-AX12 *partiePorteDroite;
-AX12 *multipleDroite;
-
-Serial pc(USBTX, USBRX);
-
Timer t;
-Ticker flipper;
-unsigned char action_precedente = 0;
-
-
-////////////////////// TABLEAU PINCE CENTRALE ///////////////////////////
-static char TAB1[20]= {0x05,0x15, 0x02, 0xFF, 0x00, ///Position initiale
- 0x06,0x00, 0x02, 0xFF, 0x00,
- 0x07,0x90, 0x01, 0xB1, 0x00,
- 0x08,0x58, 0x02, 0x79, 0x00};
-
-static char TAB2[20]= {0x05,0x50, 0x00, 0xFF, 0x02, ///Preparation prise
- 0x06,0x50, 0x01, 0xFF, 0x02,
- 0x07,0xF4, 0x01, 0xFF, 0x02,
- 0x08,0xF4, 0x01, 0xFF, 0x02};
-
-static char TAB3[20]= {0x05,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee)
- 0x06,0x50, 0x01, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-static char TAB4[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air)
- 0x06,0x50, 0x01, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB5[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
- 0x06,0xF4, 0x01, 0xFF, 0x00,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB6[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
- 0x06,0xF4, 0x01, 0xFF, 0x03,
- 0x07,0x4d, 0x01, 0xFF, 0x03,
- 0x08,0x9a, 0x02, 0xFF, 0x03};
-
-static char TAB7[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air)
- 0x06,0xB0, 0x00, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x00,
- 0x08,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB8[20]= {0x05,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas
- 0x06,0xF4, 0x01, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x00,
- 0x08,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB9[20]= {0x05,0x40, 0x00, 0xFF, 0x03, ///Deposer
- 0x06,0xF4, 0x01, 0xFF, 0x03,
- 0x07,0xD0, 0x00, 0xFF, 0x00,
- 0x08,0x35, 0x03, 0xFF, 0x00};
-
-static char TAB10[20]= {0x05,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
- 0x06,0x00, 0x01, 0xFF, 0x00,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB11[20]= {0x05,0x60, 0x00, 0xFF, 0x03, ///Pousser_module
- 0x06,0xF4, 0x01, 0xFF, 0x03,
- 0x07,0xC5, 0x00, 0xFF, 0x00,
- 0x08,0x4D, 0x03, 0xFF, 0x00};
-
-static char TAB12[20]= {0x05,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale
- 0x06,0x00, 0x02, 0xFF, 0x03,
- 0x07,0xF4, 0x01, 0xFF, 0x03,
- 0x08,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB13[20]= {0x05,0xF4, 0x00, 0xFF, 0x03, ///Deposer
- 0x06,0xA0, 0x02, 0xFF, 0x03,
- 0x07,0xD0, 0x00, 0xFF, 0x00,
- 0x08,0x35, 0x03, 0xFF, 0x00};
-
-static char TAB14[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte)
- 0x06,0x42, 0x00, 0xFF, 0x03,
- 0x07,0x4d, 0x01, 0xFF, 0x03,
- 0x08,0x9a, 0x02, 0xFF, 0x03};
+unsigned char action_precedente = 0;
+
+ /* DECLARATION VARIABLES */
-static char TAB15[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (module sur tige)
- 0x06,0x42, 0x00, 0xFF, 0x00,
- 0x07,0xC5, 0x00, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-
-static char TAB16[20]= {0x05,0x15, 0x02, 0xFF, 0x03, ///Stockage haut (pince ouverte)
- 0x06,0x42, 0x00, 0xFF, 0x00,
- 0x07,0x2c, 0x01, 0xFF, 0x03,
- 0x08,0x4D, 0x03, 0xFF, 0x03};
-////////////////////// TABLEAU BRAS GAUCHE ///////////////////////////
-static char TAB21[10]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale
- 0x02,0xF4, 0x01, 0xFF, 0x03};
-
-static char TAB22[10]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner
- 0x02,0x40, 0x03, 0xFF, 0x03};
-
-static char TAB23[10]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module
- 0x02,0xE5, 0x02, 0xFF, 0x03};
-
-
-
-
- /* ANGLE */
-
-/* 10° = 0x21, 0x00 | 110°= 0x6E, 0x01 | 210°= 0xBC, 0x02
- 20° = 0x42, 0x00 | 120°= 0x90, 0x01 | 220°= 0xDD, 0x02
- 30° = 0x64, 0x00 | 130°= 0xB1, 0x01
- 40° = 0x85, 0x00 | 140°= 0xD2, 0x01
- 50° = 0xA6, 0x00 | 150°= 0xF4, 0x01
- 60° = 0xC8, 0x00 | 160°= 0x15, 0x02
- 70° = 0xE9, 0x00 | 170°= 0x36, 0x02
- 80° = 0x0A, 0x01 | 180°= 0x58, 0x02
- 90° = 0x2C, 0x01 | 190°= 0x79, 0x02
- 100°= 0x4D, 0x01 | 200°= 0x9A, 0x02 */
-
- /* NUMERO AX12 */
-
-/* 0 = 0x00 | 9 = 0x09 | 18 = 0x12
- 1 = 0x01 | 10 = 0x0A
- 2 = 0x02 | 11 = 0x0B
- 3 = 0x03 | 12 = 0x0C
- 4 = 0x04 | 13 = 0x0D
- 5 = 0x05 | 14 = 0x0E
- 6 = 0x06 | 15 = 0x0F
- 7 = 0x07 | 16 = 0x10
- 8 = 0x08 | 17 = 0x11 */
-
-
-
-
-
-void declarationAX12(void){
- //Pince centrale
- partieBasseCentre = new AX12(p9, p10, 5, 1000000);
- partieMainGaucheCentre = new AX12(p9, p10, 7, 1000000);
- partieMainDroiteCentre = new AX12(p9, p10, 8, 1000000);
- partiePoignetCentre = new AX12(p9, p10, 6, 1000000);
- //doigt central
- partieDoigtCentre = new AX12(p9, p10, 4, 1000000);
-
- multipleCentre = new AX12(p9,p10,0xFE,1000000);
-
- //Bras de gauche
- partieBasseGauche = new AX12(p13, p14, 1, 1000000);
- partieMainGauche = new AX12(p13, p14, 2, 1000000);
- //Porte gauche
- partiePorteGauche = new AX12(p13, p14, 3, 1000000);
-
- multipleGauche = new AX12(p13,p14,0xFE,1000000);
-
- //Bras de droite
- partieBasseDroite = new AX12(p28, p27, 1, 1000000);
- partieMainDroite = new AX12(p28, p27, 2, 1000000);
- //Porte droite
- partiePorteDroite = new AX12(p28, p27, 3, 1000000);
+extern unsigned char FlagAx12;
- multipleDroite = new AX12(p28,p27,0xFE,1000000);
-
-
-
-
-
- }
-
-
-void initAX12()
-{
- declarationAX12();
-
- // init des AX12 de la partie CENTRALE du robot
- partieBasseCentre-> Set_Goal_speed(VITESSE); // VITESSE (0-1023)
-
- partieMainGauche-> Set_Goal_speed(VITESSE);
- partieMainDroite-> Set_Goal_speed(VITESSE);
- partiePoignetCentre-> Set_Goal_speed(VITESSE);
- partieDoigtCentre-> Set_Goal_speed(VITESSE);
-
- partieBasseCentre-> Set_Mode(0);
- partieMainGauche-> Set_Mode(0);
- partieMainDroite-> Set_Mode(0);
- partiePoignetCentre-> Set_Mode(0);
- partieDoigtCentre-> Set_Mode(0);
-
- // init des AX12 de ela partie GAUCHE du robot
- partieBasseGauche-> Set_Goal_speed(VITESSE);
- partieMainGauche-> Set_Goal_speed(VITESSE);
- partiePorteGauche-> Set_Goal_speed(VITESSE);
-
- partieBasseGauche-> Set_Mode(0);
- partieMainGauche-> Set_Mode(0);
- partiePorteGauche-> Set_Mode(0);
-
- // init des AX12 de ela partie DROITE du robot
- partieBasseDroite-> Set_Goal_speed(VITESSE);
- partieMainDroite-> Set_Goal_speed(VITESSE);
- //partiePorteDroite-> Set_Goal_speed(VITESSE);
-
- partieBasseDroite-> Set_Mode(0);
- partieMainDroite-> Set_Mode(0);
- //partiePorteDroite-> Set_Mode(0);
-
-
-}
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_position */
-/* DESCRIPTION : Fonction qui place les bras en position verticale */
-/****************************************************************************************/
-void Initialisation_position(void){
- multipleCentre->multiple_goal_and_speed(5,TAB12);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB1);
- //wait(TIME);
- multipleGauche->multiple_goal_and_speed(3,TAB21);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise */
-/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
-/****************************************************************************************/
-void Preparation_prise(void){
- /*if (action_precedente == 0){
- multipleCentre->multiple_goal_and_speed(5,TAB12);
- wait(TIME);
- action_precedente = 1;
- }*/
- multipleCentre->multiple_goal_and_speed(5,TAB2);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_haut */
-/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
-/****************************************************************************************/
-void Stockage_haut(void){
- multipleCentre->multiple_goal_and_speed(5,TAB3);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB4);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB5);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB6);
- wait(TIME);
-}
+extern DigitalOut led2;
+extern Serial pc;
+extern Timer t;
+extern void GetPositionAx12(void);
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_bas */
-/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
-/****************************************************************************************/
-void Stockage_bas(void){
- multipleCentre->multiple_goal_and_speed(5,TAB3);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB7);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Deposer */
-/* DESCRIPTION : Fonction qui permet de déposer le module */
-/****************************************************************************************/
-void Deposer(void){
- multipleCentre->multiple_goal_and_speed(5,TAB9);
- wait(TIME/5);
- multipleCentre->multiple_goal_and_speed(5,TAB8);
- wait(TIME/5);
- //multipleCentre->multiple_goal_and_speed(5,TAB13);
- wait(TIME/5);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depot_bas */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
-/****************************************************************************************/
-void Preparation_depot_bas(void){
- multipleCentre->multiple_goal_and_speed(5,TAB8);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depot_haut */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
-/****************************************************************************************/
-void Preparation_depot_haut(void){
- multipleCentre->multiple_goal_and_speed(5,TAB14);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB16);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB15);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB10);
- wait(TIME);
- multipleCentre->multiple_goal_and_speed(5,TAB8);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Pousser_module */
-/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
-/****************************************************************************************/
-void Pousser_module(void){
- multipleCentre->multiple_goal_and_speed(5,TAB11);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_gauche */
-/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */
-/****************************************************************************************/
-void Initialisation_gauche(void){
- //trois_myAX12_2->Set_Secure_Goal(235);
- multipleGauche->multiple_goal_and_speed(5,TAB22);
- wait(TIME);
- multipleGauche->multiple_goal_and_speed(5,TAB21);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise_gauche */
-/* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */
-/****************************************************************************************/
-void Preparation_prise_gauche(void){
- //trois_myAX12_2->Set_Secure_Goal(120);
- //multiplePorteGauche->multiple_goal_and_speed(5,TAB);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Prendre_module_gauche */
-/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
-/****************************************************************************************/
-void Prendre_module_gauche(void){
- //trois_myAX12_2->Set_Secure_Goal(160);
- //multiplePorteGauche->multiple_goal_and_speed(5,TAB);
-}
-
-/***************************************************************************************/
-/* FUNCTION NAME: RangerBrasGauche */
-/* DESCRIPTION : Fonction range le bras gauche */
-/****************************************************************************************/
-void RangerBrasGauche(void){
- multipleGauche->multiple_goal_and_speed(3,TAB21);
- wait(TIME);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_module_gauche */
-/* DESCRIPTION : Fonction qui prepare la tournante */
-/****************************************************************************************/
-void Preparation_module_gauche(void){
- multipleGauche->multiple_goal_and_speed(5,TAB22);
- wait(TIME);
-}
+AX12 *BaseBrasCentralPR, *CoudeBrasCentralPR, *PinceDBrasCentralPR, *PinceGBrasCentralPR, *DoigtBrasCentralPR, *BrasCentralPRAx12, *TabBrasCentralPR,
+ *CrocBrasGauchePR, *EpauleBrasGauchePR, *CoudeBrasGauchePR, *BrasGauchePRAx12, *TabBrasGauchePR,
+ *CrocBrasDroitPR, *EpauleBrasDroitPR, *CoudeBrasDroitPR, *BrasDroitPRAx12, *TabBrasDroitPR;
+
+
/****************************************************************************************/
/* FUNCTION NAME: Tourner_module_gauche */
/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
/****************************************************************************************/
void Tourner_module_gauche(void){
- //#define POSITION_TOURNER 0xE5
- //#define POSITION_MAX 0xFF
- /*int position = POSITION_TOURNER;
- TAB23[6] = POSITION_TOURNER;*/
-
- //multipleGauche->multiple_goal_and_speed(5,TAB23);
- wait(TIME);
- /*while((dataPressionGauche == false)&&(position < POSITION_MAX)){
- position++;
- TAB23[6] = position;
- multipleGauche->multiple_goal_and_speed(5,TAB23);
- }*/
-
while(dataCouleurGauche() == false){
printf("ici");
moteurDroitPWM(0.2);
}
moteurDroitPWM(0);
}
-
-
-void getPosiotionCentrale(void){
- pc.printf("\n\r * pince avant * \n\r");
-
- pc.printf("base : %lf \n\r ", partieBasseCentre->Get_Position() );
- pc.printf("coude : %lf \n\r ", partiePoignetCentre->Get_Position() );
- pc.printf("doigtG: %lf \n\r ", partieMainGaucheCentre->Get_Position() );
- pc.printf("doigtD: %lf \n\n\r ", partieMainDroiteCentre->Get_Position() );
-
- wait(0.2);
-}
-
-void getPosiotionGauche(void){
- pc.printf("\n\r * bras gauche *\n\r");
-
- pc.printf("epaule : %lf \n\r ",partieBasseGauche->Get_Position());
- pc.printf("main : %lf \n\r ",partieMainGauche->Get_Position() );
- pc.printf("porte : %lf \n\r ",partiePorteGauche->Get_Position() );
-
- wait(0.2);
-}
-
-void getPosiotionDroite(void){
- pc.printf("\n\r * bras droit *\n\r");
-
- pc.printf("epaule : %lf \n\r ",partieBasseDroite->Get_Position());
- pc.printf("main : %lf \n\r ",partieMainDroite->Get_Position() );
- pc.printf("porte : %lf \n\r ",partiePorteDroite->Get_Position() );
-
-
- wait(0.2);
-}
-
-
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_module_droit */
+/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
+/****************************************************************************************/
+void Tourner_module_droit(void){
+ while(dataCouleurDroit() == false){
+ printf("ici");
+ moteurGauchePWM(0.2);
+ }
+ moteurGauchePWM(0);
+}
/********************************************************************************************************/
@@ -459,4 +89,487 @@
motDroit.period(T_MOT);
motGauche.write(0.0);
motDroit.write(0.0);
-}
\ No newline at end of file
+}
+
+void initialisation_AX12(void)
+{
+ short vitesse=700;
+
+ BaseBrasCentralPR = new AX12(p9, p10, 5, 1000000);
+ CoudeBrasCentralPR = new AX12(p9, p10, 6, 1000000);
+ PinceDBrasCentralPR = new AX12(p9, p10, 8, 1000000);
+ PinceGBrasCentralPR = new AX12(p9, p10, 7, 1000000);
+ DoigtBrasCentralPR = new AX12(p9, p10, 4, 1000000);
+
+ CrocBrasGauchePR = new AX12(p13, p14, 3, 1000000);
+ CoudeBrasGauchePR = new AX12(p13, p14, 2, 1000000);
+ EpauleBrasGauchePR = new AX12(p13, p14, 1, 1000000);
+
+ CrocBrasDroitPR = new AX12(p28, p27, 11, 1000000);
+ CoudeBrasDroitPR = new AX12(p28, p27, 10, 1000000);
+ EpauleBrasDroitPR = new AX12(p28, p27, 9, 1000000);
+
+ BrasCentralPRAx12 = new AX12(p9,p10,0xFE,1000000);
+ BrasGauchePRAx12 = new AX12(p13,p14,0xFE,1000000);
+ BrasDroitPRAx12 = new AX12(p28,p27,0xFE,1000000);
+
+ BaseBrasCentralPR->Set_Goal_speed(vitesse);
+ CoudeBrasCentralPR->Set_Goal_speed(vitesse);
+ PinceDBrasCentralPR->Set_Goal_speed(vitesse);
+ PinceGBrasCentralPR->Set_Goal_speed(vitesse);
+ DoigtBrasCentralPR->Set_Goal_speed(vitesse);
+
+ CrocBrasGauchePR->Set_Goal_speed(vitesse);
+ CoudeBrasGauchePR->Set_Goal_speed(vitesse);
+ EpauleBrasGauchePR->Set_Goal_speed(vitesse);
+
+ CrocBrasDroitPR->Set_Goal_speed(vitesse);
+ CoudeBrasDroitPR->Set_Goal_speed(vitesse);
+ EpauleBrasDroitPR->Set_Goal_speed(vitesse);
+
+ BaseBrasCentralPR->Set_Mode(0);
+ CoudeBrasCentralPR->Set_Mode(0);
+ PinceDBrasCentralPR->Set_Mode(0);
+ PinceGBrasCentralPR->Set_Mode(0);
+ DoigtBrasCentralPR->Set_Mode(0);
+
+ CrocBrasGauchePR->Set_Mode(0);
+ CoudeBrasGauchePR->Set_Mode(0);
+ EpauleBrasGauchePR->Set_Mode(0);
+
+ CrocBrasDroitPR->Set_Mode(0);
+ CoudeBrasDroitPR->Set_Mode(0);
+ EpauleBrasDroitPR->Set_Mode(0);
+}
+
+void GetPositionAx12(void) {
+
+ printf("\n\r ");
+
+ printf("BaseC : %lf \n\r ", BaseBrasCentralPR->Get_Position() );
+ printf("CoudeC : %lf \n\r ", CoudeBrasCentralPR->Get_Position() );
+ printf("PinceCD : %lf \n\r ", PinceDBrasCentralPR->Get_Position());
+ printf("PinceCG : %lf \n\r ", PinceGBrasCentralPR->Get_Position());
+ printf("DoigtC : %lf \n\r ", DoigtBrasCentralPR->Get_Position() );
+
+ printf("EpauleG : %lf \n\r ", EpauleBrasGauchePR->Get_Position());
+ printf("CoudeG : %lf \n\r ", CoudeBrasGauchePR->Get_Position() );
+ printf("CrocG : %lf \n\r ", CrocBrasGauchePR->Get_Position() );
+
+ printf("EpauleD : %lf \n\r ", EpauleBrasDroitPR->Get_Position());
+ printf("CoudeD : %lf \n\r ", CoudeBrasDroitPR->Get_Position() );
+ printf("CrocD : %lf \n\r ", CrocBrasDroitPR->Get_Position() );
+}
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: Automate_ax12 */
+/* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
+/****************************************************************************************/
+void AX12_automate(unsigned char etat_ax12){
+
+ unsigned short speed;
+ unsigned int GoalPosDoigt, GoalPosBase, GoalPosCoude, GoalPosPinceG, GoalPosPinceD,
+ GoalPosEpauleTournanteG, GoalPosCoudeTournanteG,
+ GoalPosEpauleTournanteD, GoalPosCoudeTournanteD;
+
+ speed = 1000;
+
+ switch(etat_ax12){
+
+ case AX12_PINCE_CENTRALE_POSITION_INITIALE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosDoigt=1150;
+ GoalPosBase=1490;
+ GoalPosCoude=1470;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ break;
+
+ case AX12_PINCE_CENTRALE_PREPARATION_PRISE :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=170;
+ GoalPosCoude=1000;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ break;
+
+ case AX12_PINCE_CENTRALE_PRISE_MODULE :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=170;
+ GoalPosCoude=1000;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ break;
+
+ case AX12_PINCE_CENTRALE_STOCKAGE_HAUT :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=1300;
+ GoalPosCoude=700;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ wait(TIME*5);
+ GoalPosDoigt=90;
+ GoalPosBase=1450;//1050;
+ GoalPosCoude=700;//1528;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ wait(TIME*5);
+ GoalPosDoigt=90;
+ GoalPosBase=1450;//1050;
+ GoalPosCoude=1250;//1528;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ break;
+
+ case AX12_PINCE_CENTRALE_STOCKAGE_BAS :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=1000;
+ GoalPosCoude=443;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ break;
+
+ case AX12_PINCE_CENTRALE_PREPARATION_DEPOT :
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=639;
+ GoalPosCoude=557;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ wait(TIME*10);
+ GoalPosDoigt=90;
+ GoalPosBase=400;
+ GoalPosCoude=400;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+
+ break;
+
+ case AX12_PINCE_CENTRALE_DEPOSER :
+ //DEPOSER
+ wait_ms(TIME);
+ GoalPosDoigt=90;
+ GoalPosBase=440;
+ GoalPosCoude=440;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+
+ break;
+
+ case AX12_PINCE_CENTRALE_DEPOT_HAUT :
+ wait(TIME*5);
+ GoalPosDoigt=90;
+ GoalPosBase=1050;
+ GoalPosCoude=1528;
+ GoalPosPinceG=1090;
+ GoalPosPinceD=1930;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ wait(TIME*10);
+ GoalPosDoigt=90;
+ GoalPosBase=1050;
+ GoalPosCoude=1528;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+ wait(TIME*10);
+ GoalPosDoigt=90;
+ GoalPosBase=1100;
+ GoalPosCoude=700;
+ GoalPosPinceG=500;
+ GoalPosPinceD=2500;
+ mvtBrasCentralPR(AX12_DOIGT, speed, GoalPosDoigt, AX12_BASE, speed, GoalPosBase, AX12_COUDE, speed, GoalPosCoude, AX12_PINCEG, speed, GoalPosPinceG, AX12_PINCED, speed, GoalPosPinceD);
+
+
+ break;
+
+ case AX12_GAUCHE_CROC_OUVERT :
+ wait_ms(TIME);
+ CrocBrasGauchePR->Set_Secure_Goal(200);
+
+ break;
+
+ case AX12_GAUCHE_CROC_FERME :
+ wait_ms(TIME);
+ CrocBrasGauchePR->Set_Secure_Goal(240);
+
+ break;
+
+ case AX12_GAUCHE_CROC_INITIALE :
+ wait_ms(TIME);
+ CrocBrasGauchePR->Set_Secure_Goal(300);
+
+ break;
+
+ case AX12_TOURNANTE_GAUCHE_POSITION_INITIALE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteG=1450;
+ GoalPosEpauleTournanteG=600;
+ mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
+
+
+ break;
+
+ case AX12_TOURNANTE_GAUCHE_PREPARATION :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteG=930;
+ GoalPosEpauleTournanteG=1962;
+ mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
+
+
+ break;
+
+ case AX12_TOURNANTE_GAUCHE_MODULE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteG=894;
+ GoalPosEpauleTournanteG=2200;
+ mvtBrasGauchePR(AX12_GAUCHE_EPAULE, speed, GoalPosEpauleTournanteG, AX12_GAUCHE_COUDE, speed, GoalPosCoudeTournanteG);
+
+
+ break;
+
+ case AX12_DROIT_CROC_OUVERT :
+ wait_ms(TIME);
+ CrocBrasDroitPR->Set_Secure_Goal(106);
+
+ break;
+
+ case AX12_DROIT_CROC_FERME :
+ wait_ms(TIME);
+ CrocBrasDroitPR->Set_Secure_Goal(55);
+
+ break;
+
+ case AX12_DROIT_CROC_INITIALE :
+ wait_ms(TIME);
+ CrocBrasDroitPR->Set_Secure_Goal(0);
+
+ break;
+
+ case AX12_TOURNANTE_DROIT_POSITION_INITIALE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteD=1610;
+ GoalPosEpauleTournanteD=2337;
+ mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
+
+
+ break;
+
+ case AX12_TOURNANTE_DROIT_PREPARATION :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteD=930;
+ GoalPosEpauleTournanteD=1962;
+ mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
+
+
+ break;
+
+ case AX12_TOURNANTE_DROIT_MODULE :
+ wait_ms(TIME);
+ speed = 511;
+ GoalPosCoudeTournanteD=894;
+ GoalPosEpauleTournanteD=2200;
+ mvtBrasDroitPR(AX12_DROIT_EPAULE, speed, GoalPosEpauleTournanteD, AX12_DROIT_COUDE, speed, GoalPosCoudeTournanteD);
+
+
+ break;
+
+ case AX12_DEFAUT :
+ break;
+
+ case AX12_POSITION :
+ GetPositionAx12();
+ break;
+ }
+}
+
+void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
+ unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
+ unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
+ unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5)
+{
+ char TabBrasCentralPR[25];
+ unsigned short GPosition1_1, GPosition2_1, GPosition3_1, GPosition4_1, GPosition5_1;
+ Timer timeOut;
+
+
+ GPosition1_1=((unsigned long)GPosition1*341/1000);
+ GPosition2_1=((unsigned long)GPosition2*341/1000);
+ GPosition3_1=((unsigned long)GPosition3*341/1000);
+ GPosition4_1=((unsigned long)GPosition4*341/1000);
+ GPosition5_1=((unsigned long)GPosition5*341/1000);
+
+ TabBrasCentralPR[0] = ID1;
+ TabBrasCentralPR[1] = GPosition1_1;
+ TabBrasCentralPR[2] = GPosition1_1>>8;
+ TabBrasCentralPR[3] = GSpeed1;
+ TabBrasCentralPR[4] = GSpeed1>>8;
+
+ TabBrasCentralPR[5] = ID2;
+ TabBrasCentralPR[6] = GPosition2_1;
+ TabBrasCentralPR[7] = GPosition2_1>>8;
+ TabBrasCentralPR[8] = GSpeed2;
+ TabBrasCentralPR[9] = GSpeed2>>8;
+
+ TabBrasCentralPR[10] = ID3;
+ TabBrasCentralPR[11] = GPosition3_1;
+ TabBrasCentralPR[12] = GPosition3_1>>8;
+ TabBrasCentralPR[13] = GSpeed3;
+ TabBrasCentralPR[14] = GSpeed3>>8 ;
+
+ TabBrasCentralPR[15] = ID4;
+ TabBrasCentralPR[16] = GPosition4_1;
+ TabBrasCentralPR[17] = GPosition4_1>>8;
+ TabBrasCentralPR[18] = GSpeed4;
+ TabBrasCentralPR[19] = GSpeed4>>8 ;
+
+ TabBrasCentralPR[20] = ID5;
+ TabBrasCentralPR[21] = GPosition5_1;
+ TabBrasCentralPR[22] = GPosition5_1>>8;
+ TabBrasCentralPR[23] = GSpeed5;
+ TabBrasCentralPR[24] = GSpeed5>>8 ;
+
+
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ wait(TIME);
+
+ timeOut.start;
+ while (((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)>GPosition1*110/100) || ((unsigned short)(DoigtBrasCentralPR->Get_Position()*10)<GPosition1*90/100) || (timeOut.read_ms() > 100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ wait(TIME);
+
+ }
+
+ timeOut.reset;
+ while (((unsigned short)(BaseBrasCentralPR->Get_Position()*10)>GPosition2*110/100) || ((unsigned short)(BaseBrasCentralPR->Get_Position()*10)<GPosition2*90/100)|| (timeOut.read_ms() > 100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ wait(TIME);
+ }
+
+ timeOut.reset;
+ while (((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)>GPosition3*110/100) || ((unsigned short)(CoudeBrasCentralPR->Get_Position()*10)<GPosition3*90/100)|| (timeOut.read_ms() > 100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ wait(TIME);
+ }
+
+ timeOut.reset;
+ while (((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)>GPosition4*110/100) || ((unsigned short)(PinceGBrasCentralPR->Get_Position()*10)<GPosition4*90/100)|| (timeOut.read_ms() > 100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ wait(TIME);
+ }
+
+ timeOut.reset;
+ while (((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)>GPosition5*110/100) || ((unsigned short)(PinceDBrasCentralPR->Get_Position()*10)<GPosition5*90/100)|| (timeOut.read_ms() > 100)) {
+ BrasCentralPRAx12->multiple_goal_and_speed(5,TabBrasCentralPR) ;
+ wait(TIME);
+ }
+
+
+}
+
+
+void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
+{
+ char TabBrasGauchePR[10];
+ unsigned short GPosition1_1, GPosition2_1;
+
+ GPosition1_1=((unsigned long)GPosition1*341/1000);
+ GPosition2_1=((unsigned long)GPosition2*341/1000);
+
+ TabBrasGauchePR[0] = ID1;
+ TabBrasGauchePR[1] = GPosition1_1;
+ TabBrasGauchePR[2] = GPosition1_1>>8;
+ TabBrasGauchePR[3] = GSpeed1;
+ TabBrasGauchePR[4] = GSpeed1>>8;
+
+ TabBrasGauchePR[5] = ID2;
+ TabBrasGauchePR[6] = GPosition2_1;
+ TabBrasGauchePR[7] = GPosition2_1>>8;
+ TabBrasGauchePR[8] = GSpeed2;
+ TabBrasGauchePR[9] = GSpeed2>>8;
+
+
+ BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
+ wait(TIME);
+
+ while (((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(EpauleBrasGauchePR->Get_Position()*10)<GPosition1*95/100)) {
+ BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
+ wait(TIME*5);
+ }
+
+ while (((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(CoudeBrasGauchePR->Get_Position()*10)<GPosition2*95/100)) {
+ BrasGauchePRAx12->multiple_goal_and_speed(2,TabBrasGauchePR) ;
+ wait(TIME*5);
+ }
+}
+
+void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2)
+{
+ char TabBrasDroitPR[10];
+ unsigned short GPosition1_1, GPosition2_1;
+
+ GPosition1_1=((unsigned long)GPosition1*341/1000);
+ GPosition2_1=((unsigned long)GPosition2*341/1000);
+
+ TabBrasDroitPR[0] = ID1;
+ TabBrasDroitPR[1] = GPosition1_1;
+ TabBrasDroitPR[2] = GPosition1_1>>8;
+ TabBrasDroitPR[3] = GSpeed1;
+ TabBrasDroitPR[4] = GSpeed1>>8;
+
+ TabBrasDroitPR[5] = ID2;
+ TabBrasDroitPR[6] = GPosition2_1;
+ TabBrasDroitPR[7] = GPosition2_1>>8;
+ TabBrasDroitPR[8] = GSpeed2;
+ TabBrasDroitPR[9] = GSpeed2>>8;
+
+
+ BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
+ wait(TIME);
+
+ while (((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)>GPosition1*105/100) || ((unsigned short)(EpauleBrasDroitPR->Get_Position()*10)<GPosition1*95/100)) {
+ BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
+ wait(TIME*5);
+ }
+
+ while (((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)>GPosition2*105/100) || ((unsigned short)(CoudeBrasDroitPR->Get_Position()*10)<GPosition2*95/100)) {
+ BrasDroitPRAx12->multiple_goal_and_speed(2,TabBrasDroitPR) ;
+ wait(TIME*5);
+ }
+}
+
--- a/peripheriques/capteurs.cpp Fri May 19 17:14:07 2017 +0000
+++ b/peripheriques/capteurs.cpp Mon May 22 15:01:49 2017 +0000
@@ -27,8 +27,9 @@
return couleurOK;
}
-short dataTelemetre(void){
- return telemetre.read_u16();
+unsigned short dataTelemetre(void){
+ float distance = telemetre.read()*3.3*1159.6-687.5+98;
+ return (unsigned short)distance;
}
bool dataPressionGauche(void){
--- a/peripheriques/peripheriques.h Fri May 19 17:14:07 2017 +0000
+++ b/peripheriques/peripheriques.h Mon May 22 15:01:49 2017 +0000
@@ -4,31 +4,83 @@
#include "global.h"
#define VITESSE 700
+#define TIME 0.01
+#define T_MOT 0.00005
-/*
-DigitalIn IO1(p23);
-DigitalIn IO2(p24);
-DigitalIn IO3(p25);
-DigitalIn IO4(p26);
+
+#define AX12_PINCE_CENTRALE_POSITION_INITIALE 1
+#define AX12_PINCE_CENTRALE_PREPARATION_PRISE 2
+#define AX12_PINCE_CENTRALE_PRISE_MODULE 3
+#define AX12_PINCE_CENTRALE_STOCKAGE_HAUT 4
+#define AX12_PINCE_CENTRALE_STOCKAGE_BAS 5
+#define AX12_PINCE_CENTRALE_PREPARATION_DEPOT 6
+#define AX12_PINCE_CENTRALE_DEPOSER 7
+#define AX12_PINCE_CENTRALE_DEPOT_HAUT 8
+
+#define AX12_GAUCHE_CROC_OUVERT 11
+#define AX12_GAUCHE_CROC_FERME 12
+#define AX12_DROIT_CROC_INITIALE 13
+
+
+#define AX12_DROIT_CROC_OUVERT 14
+#define AX12_DROIT_CROC_FERME 15
+#define AX12_GAUCHE_CROC_INITIALE 16
+
+#define AX12_TOURNANTE_GAUCHE_POSITION_INITIALE 21
+#define AX12_TOURNANTE_GAUCHE_PREPARATION 22
+#define AX12_TOURNANTE_GAUCHE_MODULE 23
+
+#define AX12_TOURNANTE_DROIT_POSITION_INITIALE 24
+#define AX12_TOURNANTE_DROIT_PREPARATION 25
+#define AX12_TOURNANTE_DROIT_MODULE 26
+
+#define AX12_POSITION 100
+#define AX12_DEFAUT 0
-AnalogIn A_in1(p15);
-AnalogIn A_in2(p16);
-AnalogIn A_in3(p17);
-AnalogIn A_in4(p18);
-AnalogIn A_in5(p19);
-AnalogIn A_in6(p20);
+#define AX12_DOIGT 4
+#define AX12_BASE 5
+#define AX12_COUDE 6
+#define AX12_PINCEG 7
+#define AX12_PINCED 8
+#define AX12_GAUCHE_EPAULE 1
+#define AX12_GAUCHE_COUDE 2
+#define AX12_DROIT_EPAULE 9
+#define AX12_DROIT_COUDE 10
+
+#define TOLERANCE_AX12 50
+
+ /* PROTOTYPES DE FONCTIONS ET POINTEURS */
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: Fin_action */
+/* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
+/****************************************************************************************/
+void Fin_action(void);
-PwmOut IRL_1(p21);
-PwmOut IRL_2(p22);
-*/
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_position */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+void Initialisation_position(unsigned char choix);
-#define TIME 1
-#define T_MOT 0.00005
-/*********************************************************************************************************/
-/* FUNCTION NAME: initAX12 */
-/* DESCRIPTION : initialise les AX12 */
-/*********************************************************************************************************/
-void initAX12(void);
+void mvtBrasCentralPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2,
+ unsigned char ID3, unsigned short GSpeed3, unsigned short GPosition3,
+ unsigned char ID4, unsigned short GSpeed4, unsigned short GPosition4,
+ unsigned char ID5, unsigned short GSpeed5, unsigned short GPosition5);
+
+void mvtBrasGauchePR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2);
+
+void mvtBrasDroitPR(unsigned char ID1, unsigned short GSpeed1, unsigned short GPosition1,
+ unsigned char ID2, unsigned short GSpeed2, unsigned short GPosition2);
+
+void initialisation_AX12(void);
+
+void AX12_automate(unsigned char etat_ax12);
/*********************************************************************************************************/
/* FUNCTION NAME: moteurGauchePWM */
@@ -50,80 +102,6 @@
/* PROTOTYPES DE FONCTIONS ET POINTEURS */
-void declarationAX12(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_position */
-/* DESCRIPTION : Fonction qui place les bras en position verticale */
-/****************************************************************************************/
-void Initialisation_position(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise */
-/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
-/****************************************************************************************/
-void Preparation_prise(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_haut */
-/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
-/****************************************************************************************/
-void Stockage_haut(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Stockage_bas */
-/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
-/****************************************************************************************/
-void Stockage_bas(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Deposer */
-/* DESCRIPTION : Fonction qui permet de déposer un module */
-/****************************************************************************************/
-void Deposer(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depos_bas */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
-/****************************************************************************************/
-void Preparation_depot_bas(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_depos_haut */
-/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
-/****************************************************************************************/
-void Preparation_depot_haut(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Pousser_module */
-/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
-/****************************************************************************************/
-void Pousser_module(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Initialisation_gauche */
-/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */
-/****************************************************************************************/
-void Initialisation_gauche(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Preparation_prise_gauche */
-/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
-/****************************************************************************************/
-void Preparation_prise_gauche(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: Prendre_module_gauche */
-/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
-/****************************************************************************************/
-void Prendre_module_gauche(void);
-
-/***************************************************************************************/
-/* FUNCTION NAME: RangerBrasGauche */
-/* DESCRIPTION : Fonction range le bras gauche */
-/****************************************************************************************/
-void RangerBrasGauche(void);
-
/****************************************************************************************/
/* FUNCTION NAME: Tourner_module_gauche */
/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
@@ -131,24 +109,16 @@
void Tourner_module_gauche(void);
/****************************************************************************************/
-/* FUNCTION NAME: Preparatio_module_gauche */
-/* DESCRIPTION : Fonction qui prepare le tournante */
+/* FUNCTION NAME: Tourner_module_droit */
+/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
/****************************************************************************************/
-void Preparation_module_gauche(void);
-
-void getPosiotionCentrale(void);
-void getPosiotionGauche(void);
-void getPosiotionDroite(void);
-
-
-
-
+void Tourner_module_droit(void);
bool dataCouleurGauche(void);
bool dataCouleurDroit(void);
-short dataTelemetre(void);
+unsigned short dataTelemetre(void);
bool dataPressionGauche(void);
bool dataPressionDroit(void);
-#endif
\ No newline at end of file
+#endif