CRAC Team
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
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();*/
+
}
/****************************************************************************************/