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
Diff: Robots/Strategie_small.cpp
- Revision:
- 18:cc5fec34ed9c
- Parent:
- 16:7321fb3bb396
- Child:
- 21:590cdacb6a35
--- 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();*/
+
}
/****************************************************************************************/