CRAC Team
code avec modifs, programme mit dans les robots pour les derniers matchs
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Revision 14:6aa8aa1699ad, committed 2020-03-11
- Comitter:
- gabrieltetar
- Date:
- Wed Mar 11 14:21:02 2020 +0000
- Parent:
- 11:848ea39838bf
- Child:
- 15:5ece9c3ba389
- Commit message:
- 11/03/2020
Changed in this revision
--- a/Debug/debug.cpp Wed Mar 04 12:38:22 2020 +0000
+++ b/Debug/debug.cpp Wed Mar 11 14:21:02 2020 +0000
@@ -2,11 +2,10 @@
//debug audio
void Debug_Audio(unsigned char repertoire, unsigned char track){
- unsigned char data[3];
- data[0]=0x01;
- data[1]=repertoire;
- data[2]=track;
- SendMsgCan(SOMO, data,3);
+ unsigned char data[2];
+ data[0]=repertoire;
+ data[1]=track;
+ SendMsgCan(SOMO_playtrack, data,2);
}
//azezaea
--- a/Globals/global.h Wed Mar 04 12:38:22 2020 +0000 +++ b/Globals/global.h Wed Mar 11 14:21:02 2020 +0000 @@ -9,12 +9,10 @@ #include <DirHandle.h> #include "SDFileSystem.h" #include "conv_data.h" -#include "Config_big.h" #include "constantes.h" #include "Instruction.h" #include "Strategie.h" #include "lecture_repertoire.h" -#include "StrategieManager.h" #include "SerialHalfDuplex.h" #include "debug.h" #include "Asservissement.h" @@ -22,7 +20,6 @@ #include "LCD_DISCO_F469NI.h" #include "fonts.h" #include "F469_GUI.hpp" -#include "Config_big.h" #include "LiaisonBluetooth.h" #include "Evitement.h"
--- a/Globals/ident_crac.lib Wed Mar 04 12:38:22 2020 +0000 +++ b/Globals/ident_crac.lib Wed Mar 11 14:21:02 2020 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#b3e08bf1288d +https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#fc849cd9baa8
--- a/Robots/Config_big.h Wed Mar 04 12:38:22 2020 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,61 +0,0 @@ -#ifndef CRAC_CONFIG_BIG -#define CRAC_CONFIG_BIG -/** -Fichier de configuration des action specifique au gros robot -**/ -#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 -#define AX12_ID_PINCE_ARRIERE_HAUTE_DROITE 15 -#define AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE 13 -#define AX12_ID_PINCE_ARRIERE_BASSE_DROITE 14 -#define AX12_ID_PORTE_ARRIERE_DROITE 4 -#define AX12_ID_PORTE_ARRIERE_GAUCHE 5 -#define AX12_ID_FUNNY_ACTION 1 -#define AX12_ID_CHARIOT 2 -#define AX12_ID_PEIGNE 3 -#define AX12_ID_PORTE_AVANT_DROITE 11 -#define AX12_ID_PORTE_AVANT_GAUCHE 6 -#define AX12_ID_VENTOUSE 8 -#define AX12_ID_CONE 7 - -#define AX12_ANGLE_FUNNY_ACTION_CLOSE 150 -#define AX12_ANGLE_FUNNY_ACTION_OPEN 95 - - - -#define AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE 18 -#define AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER 120 -#define AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE 280 -#define AX12_ANGLE_PORTE_AVANT_DROITE_FERMER 183 - -#define AX12_ANGLE_PEIGNE_UP 60 -#define AX12_ANGLE_PEIGNE_DOWN 120 - -#define AX12_ANGLE_VENTOUSE_UP 150 -#define AX12_ANGLE_VENTOUSE_DOWN 130 - -#define AX12_ANGLE_CONE_INSIDE 45 -#define AX12_ANGLE_CONE_OUTSIDE 155 - - - -#define AX12_SPEED_FUNNY_ACTION 100 -#define AX12_SPEED_VENTOUSE 100 -#define AX12_SPEED_PEIGNE 500 - -#define POMPES_PWM 100 - - - -#endif \ No newline at end of file
--- a/Robots/StrategieManager.h Wed Mar 04 12:38:22 2020 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,55 +0,0 @@ -#ifndef CRAC_STRATEGIE_BIG -#define CRAC_STRATEGIE_BIG - -#include "global.h" - -/****************************************************************************************/ -/* FUNCTION NAME: doFunnyAction */ -/* DESCRIPTION : Permet de faire la funny action en fin de partie */ -/****************************************************************************************/ -void doFunnyAction(void); - -/****************************************************************************************/ -/* FUNCTION NAME: doAction */ -/* DESCRIPTION : Effectuer une action specifique */ -/****************************************************************************************/ -unsigned char doAction(unsigned char id, unsigned short speed, short angle); - -/****************************************************************************************/ -/* FUNCTION NAME: initRobot */ -/* DESCRIPTION : initialiser le robot */ -/****************************************************************************************/ -//void initRobot(void); - -/****************************************************************************************/ -/* FUNCTION NAME: initRobotActionneur */ -/* DESCRIPTION : Initialiser la position des actionneurs du robot */ -/****************************************************************************************/ -//void initRobotActionneur(void); - -/****************************************************************************************/ -/* FUNCTION NAME: runTest */ -/* DESCRIPTION : tester l'ensemble des actionneurs du robot */ -/****************************************************************************************/ -void runRobotTest(void); - -/****************************************************************************************/ -/* FUNCTION NAME: SelectStrategy */ -/* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */ -/* RETURN : 0=> Erreur, 1=> OK si le fichier existe */ -/****************************************************************************************/ -int SelectStrategy(unsigned char id); - -/****************************************************************************************/ -/* FUNCTION NAME: needToStop */ -/* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */ -/****************************************************************************************/ -unsigned char needToStop(void); - -/****************************************************************************************/ -/* FUNCTION NAME: doBeforeEndAction */ -/* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */ -/****************************************************************************************/ -void doBeforeEndAction(void); - -#endif \ No newline at end of file
--- a/Robots/Strategie_big.cpp Wed Mar 04 12:38:22 2020 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,305 +0,0 @@
-#include "global.h"
-#ifdef ROBOT_BIG
-
-unsigned short x;
-unsigned short y;
-unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
-
-/****************************************************************************************/
-/* FUNCTION NAME: doFunnyAction */
-/* DESCRIPTION : Permet de faire la funny action en fin de partie */
-/****************************************************************************************/
-void doFunnyAction(void)
-{
- //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);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: doAction */
-/* DESCRIPTION : Effectuer une action specifique */
-/****************************************************************************************/
-unsigned char doAction(unsigned char id, unsigned short var1, short var2)
-{
- CANMessage msgTx=CANMessage();
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- //affichage_debug(id);
-
- switch(id) {
- /////////////////////////////////////////////////////////100 à 108 : ACTIONS HERKULEX/////////////////////////////////////////////
- case 118:
- SendRawId(VENTOUSE_AV_CENTRE_BALANCE);
- break;
-
- case 201:
- unsigned char var_tempo;
- var_tempo = (unsigned char)var1;//0auto 1forceon 2 forceoff
- SendMsgCan(ASCENSEUR, &var_tempo,1);
- waitingAckFrom = 0;
- waitingAckID =0;
- break;
-
- case 202:
- msgTx.id=VIDER_CONVOYEUR;
- msgTx.len=1;
- msgTx.data[0]=(unsigned char)var1; //
- can2.write(msgTx);
- break;
-
- case 203:
- x = var1;
- if(InversStrat == 1) {
- y = 3000 - var2;
- } else {
- y = var2;
- }
- Send2Short(GOLDENIUM_AVANT, x, y);
- break;
-
- case 204:
- unsigned char arg_tempo;
- if(InversStrat == 1) {
- switch(var1) {
- case AV_DROIT:
- arg_tempo = AV_GAUCHE;
- break;
- case AV_GAUCHE:
- arg_tempo = AV_DROIT;
- break;
- default :
- arg_tempo =(unsigned char)var1;
- break;
- }
-
- } else arg_tempo =(unsigned char)var1;
- SendMsgCan(HACHEUR_RELEASE_ATOM, &arg_tempo,1);
- waitingAckFrom = 0;
- waitingAckID =0;
- break;
-
- case 205:
- SendRawId(PRESENTOIR_AVANT);
- break;
-
- case 206:
- SendMsgCan(RATEAU, (unsigned char*)&var1,1);
- break;
-
-
- case 150:
- SCORE_GR+=var1;
- SCORE_GLOBAL=SCORE_GR+SCORE_PR;
- //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
-
-
- case 11://0 Désactiver le stop,1 Activer le stop saut de strat,2 Activer le stop avec evitement
- isStopEnable =(unsigned char) var1;
- // SendMsgCan(0x5BC, &isStopEnable,1);
- waitingAckFrom = 0;
- waitingAckID =0;
- break;
-
- case 20://Désactiver l'asservissement
- setAsservissementEtat(0);
- break;
-
- case 21://Activer l'asservissement
- setAsservissementEtat(1);
- break;
-
- case 22://Changer la vitesse du robot
- SendSpeed(var1);
- waitingAckFrom = 0;
- waitingAckID = 0;
- wait(0.2);
- break;
-
- case 23:
- SendAccel((unsigned short)var1,(unsigned short)var2);//,(unsigned short)arg2, (unsigned short)arg2);
- wait_us(200);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
-
- case 30://Action tempo
- wait_ms(var1);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
-
-
-
- default:
- return 0;//L'action n'existe pas, il faut utiliser le CAN
-
- }
- return 1;//L'action est spécifique.
-
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: initRobot */
-/* DESCRIPTION : initialiser le robot */
-/****************************************************************************************/
-void initRobot(void)
-{
- //Enregistrement de tous les AX12 présent sur la carte
- /*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_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
- //AX12_processChange();
- //runRobotTest();
-
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: initRobotActionneur */
-/* DESCRIPTION : Initialiser la position des actionneurs du robot */
-/****************************************************************************************/
-void initRobotActionneur(void)
-{
- /*doAction(100,1,0);
- doAction(100,2,0);
- doAction(110,0,0);
- doAction(120,0,0);
- doAction(131,0,0);*/
-
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: runTest */
-/* DESCRIPTION : tester l'ensemble des actionneurs du robot */
-/****************************************************************************************/
-void runRobotTest(void)
-{
- /*
- int waitTime = 500;
-
- //Test des AX12 dans l'ordre
- doAction(111,0,0);//Fermeture pince arrière haute
- wait_ms(waitTime);
- doAction(110,0,0);//Ouverture pince arrière haute
- wait_ms(waitTime);
- doAction(113,0,0);//Fermeture pince arrière basse
- wait_ms(waitTime);
- doAction(112,0,0);//Ouverture pince arrière basse
- wait_ms(waitTime);
- doAction(115,0,0);//Fermeture porte arrière
- wait_ms(waitTime);
- doAction(114,0,0);//Ouverture porte arrière
- wait_ms(waitTime);
- doAction(101,0,0);//Fermer les portes avant
- wait_ms(waitTime);
- doAction(100,0,0);//Ouvrir les portes avant
- wait_ms(waitTime);
- doAction(103,0,0);//Descendre le peigne
- wait_ms(waitTime);
- doAction(102,0,0);//Remonter le peigne*/
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: SelectStrategy */
-/* DESCRIPTION : Charger le fichier de stratégie correspondante à un id */
-/* RETURN : 0=> Erreur, 1=> OK si le fichier existe */
-/****************************************************************************************/
-/*int SelectStrategy(unsigned char id)
-{
- switch(id)
- {
- // strat de match
- case 1:
- strcpy(cheminFileStart,"/local/strat1.txt");
- return FileExists(cheminFileStart);
- case 2:
- strcpy(cheminFileStart,"/local/strat2.txt");
- return FileExists(cheminFileStart);
- case 3:
- strcpy(cheminFileStart,"/local/strat3.txt");
- return FileExists(cheminFileStart);
- case 4:
- strcpy(cheminFileStart,"/local/strat4.txt");
- return FileExists(cheminFileStart);
- case 5:
- strcpy(cheminFileStart,"/local/strat5.txt");
- return FileExists(cheminFileStart);
- case 6:
- strcpy(cheminFileStart,"/local/strat6.txt");
- return FileExists(cheminFileStart);
- case 7:
- strcpy(cheminFileStart,"/local/strat7.txt");
- return FileExists(cheminFileStart);
- case 8:
- strcpy(cheminFileStart,"/local/strat8.txt");
- return FileExists(cheminFileStart);
- case 9:
- strcpy(cheminFileStart,"/local/strat9.txt");
- return FileExists(cheminFileStart);
- case 10:
- strcpy(cheminFileStart,"/local/strat10.txt");
- return FileExists(cheminFileStart);
-
- // strat de demo
- case 0x10:
- strcpy(cheminFileStart,"/local/moteur.txt");
- return FileExists(cheminFileStart);
- case 0x11:
-#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;
- }
-}*/
-
-/****************************************************************************************/
-/* FUNCTION NAME: needToStop */
-/* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */
-/****************************************************************************************/
-unsigned char needToStop(void)
-{
- return isStopEnable;
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: doBeforeEndAction */
-/* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */
-/****************************************************************************************/
-void doBeforeEndAction(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
-}
-
-#endif
--- a/Robots/Strategie_small.cpp Wed Mar 04 12:38:22 2020 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,235 +0,0 @@
-#include "global.h"
-
-#ifdef ROBOT_SMALL
-
-unsigned short distance_recalage;
-unsigned short distance_revenir;
-
-unsigned short x;
-unsigned short y;
-unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
-//unsigned short telemetreDistance;
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: doFunnyAction */
-/* DESCRIPTION : Permet de faire la funny action en fin de partie */
-/****************************************************************************************/
-void doFunnyAction(void)
-{
-
-
-}
-
-
-/*************************************************************************************************/
-/* FUNCTION NAME: doAction */
-/* DESCRIPTION : Effectuer une action specifique correspondant au numéro dans le fichier strat */
-/*************************************************************************************************/
-unsigned char doAction(unsigned char id, unsigned short arg1, short arg2)
-{
- int retour = 1;
- CANMessage msgTx=CANMessage();
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- //SendMsgCan(0x5BB, &id,1);
- switch(id) {
- case 101: //bras gabarit
- SendRawId(GABARIT_PETIT_ROBOT);
- break;
- case 102: //attraper presentoir avant
- SendRawId(PRESENTOIR_AVANT);
- break;
- case 103: //attraper presentoir arriere
- SendRawId(PRESENTOIR_ARRIERE);
- break;
- case 104: //balance avant
- SendRawId(BALANCE_AVANT);
- break;
- case 105: //balance arriere
- SendRawId(BALANCE_ARRIERE);
- break;
- case 106: //accelerateur avant
- //SendRawId(ACCELERATEUR_AVANT);
- distance_recalage=arg1;
- distance_revenir=arg2;
- Send2Short(ACCELERATEUR_AVANT, distance_recalage,distance_revenir);
- break;
- case 107: //accelerateur arriere
- distance_recalage=arg1;
- distance_revenir=arg2;
- Send2Short(ACCELERATEUR_ARRIERE, distance_recalage,distance_revenir);
- break;
- case 108: //goldenium avant
- //SendRawId(GOLDENIUM_AVANT);
- x = arg1;
- if(InversStrat == 1) {
- y = 3000 - arg2;
- } else {
- y = arg2;
- }
- Send2Short(GOLDENIUM_AVANT, x, y);
- break;
- case 109: //goldenium arriere
- x=arg1;
- y=arg2;
- Send2Short(GOLDENIUM_ARRIERE, x,y);
- break;
- case 110: //sol avant
- SendRawId(SOL_AVANT);
- break;
- case 111: //sol arriere
- SendRawId(SOL_ARRIERE);
- break;
- case 112: //sol avant relache
- SendRawId(SOL_AVANT_RELACHE);
- break;
- case 113: //sol arriere relache
- SendRawId(SOL_ARRIERE_RELACHE);
- break;
- case 114://relache les 3 ventouses avants
- SendRawId(AVANT_RELACHE);
- break;
-
- case 115://relache les 3 ventouses arrieres
- SendRawId(ARRIERE_RELACHE);
- break;
-
- case 116://relache une pompe
- unsigned char arg_tempo;
- if(InversStrat == 1) {
- switch(arg1) {
- case AV_DROIT:
- arg_tempo = AV_GAUCHE;
- break;
- case AV_GAUCHE:
- arg_tempo = AV_DROIT;
- break;
- case AR_DROIT:
- arg_tempo = AR_GAUCHE;
- break;
- case AR_GAUCHE:
- arg_tempo = AR_DROIT;
- break;
- default :
- arg_tempo =(unsigned char)arg1;
- break;
- }
-
- } else arg_tempo =(unsigned char)arg1;
- SendMsgCan(HACHEUR_RELEASE_ATOM, &arg_tempo,1);
- waitingAckFrom = 0;
- waitingAckID =0;
- break;
- case 117:
- SendRawId(RECROQUEVILLER);
- break;
- case 118:
- SendRawId(VENTOUSE_AV_CENTRE_BALANCE);
- break;
- case 119:
- SendRawId(VENTOUSE_AR_CENTRE_BALANCE);
- break;
- case 120:
- SendRawId(ACCELERATEUR_INSERTION_AVANT_GAUCHE);
- break;
-
- case 121:
- SendRawId(ACCELERATEUR_INSERTION_ARRIERE_GAUCHE);
- break;
-
- case 150:
- SCORE_PR+=arg1;
- liaison_Tx.envoyer_short(0x30,SCORE_PR);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
-
-
- case 200 :
- SendRawId(DATA_TELEMETRE);
- /*telemetreDistance = dataTelemetre();
- wait_ms(1);
- telemetreDistance = dataTelemetre();
- telemetreDistance = telemetreDistance - 170;*/
- break;
-
- case 201 :
- SendRawId(0x99);//
- retour = 2;
- break;
-
-
- case 11://0 Désactiver le stop,1 Activer le stop saut de strat,2 Activer le stop avec evitement
- isStopEnable =(unsigned char) arg1;
- // SendMsgCan(0x5BC, &isStopEnable,1);
- waitingAckFrom = 0;
- waitingAckID =0;
- break;
-
-
-
- case 20://Désactiver l'asservissement
- setAsservissementEtat(0);
- break;
-
- case 21://Activer l'asservissement
- setAsservissementEtat(1);
- break;
-
- case 22://Changer la vitesse du robot
- SendSpeed(arg1);//,(unsigned short)arg2, (unsigned short)arg2);
- wait_us(200);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
- case 23:
- SendAccel(arg1,(unsigned short)arg2);//,(unsigned short)arg2, (unsigned short)arg2);
- wait_us(200);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
-
-
- case 19: // CHANGER LA VITESSE + DECELERATION
- //SendSpeedDecel(arg1,(unsigned short) arg2);
- wait_us(200);
- waitingAckFrom = 0;
- waitingAckID =0;
- break;
-
- case 30://Action tempo
- wait_ms(arg1);
- waitingAckFrom = 0;
- waitingAckID = 0;
- break;
-
- default:
- retour = 0;//L'action n'existe pas, il faut utiliser le CAN
-
- }
- return retour;//L'action est spécifique.
-
-}
-
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: needToStop */
-/* DESCRIPTION : Savoir si il faut autoriser le stop du robot via balise */
-/****************************************************************************************/
-unsigned char needToStop(void)
-{
- return isStopEnable;
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: doBeforeEndAction */
-/* DESCRIPTION : Terminer les actions du robot 1s avant la fin du match */
-/****************************************************************************************/
-void doBeforeEndAction(void)
-{
-
-}
-
-#endif
--- a/Strategie/Strategie.cpp Wed Mar 04 12:38:22 2020 +0000
+++ b/Strategie/Strategie.cpp Wed Mar 11 14:21:02 2020 +0000
@@ -3,8 +3,6 @@
#include <sstream>
#include <math.h>
#include <vector>
-//#include "StrategieManager.h"
-
#define M_PI 3.14159265358979323846
@@ -72,7 +70,12 @@
short SCORE_GLOBAL=0;
short SCORE_GR=0;
short SCORE_PR=0;
+unsigned short distance_recalage;
+unsigned short distance_revenir;
+unsigned short x;
+unsigned short y;
+unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
int flag = 0, flag_strat = 0, flag_timer;
int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0;
char Ack_strat = 0;
@@ -118,7 +121,7 @@
float cptf;
int cpt,cpt1;
-typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_COULEUR, TEST_SERVO_BRAS, TEST_VENTOUSE, TEST_ELECTROV, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat;
+typedef enum {INIT, ATT, CHOIX, DEMO, TEST_MOTEUR, TEST_COULEUR, TEST_SERVO_BRAS, TEST_VENTOUSE, TEST_ELECTROV,TEST_AUD, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat;
T_etat etat = INIT;
E_stratGameEtat gameEtat = ETAT_CHECK_CARTES;
E_stratGameEtat memGameEtat= gameEtat;
@@ -135,11 +138,11 @@
Button DEMONSTRATION (0, 400, 400, 320, "Demo");
//menu demo
Button TEST_VENT(0, 25, 400, 100, "Test ventouses");
-Button TEST_TELE(0, 135, 400, 100, "Test telemetre");
+Button TEST_MOT(0, 135, 400, 100, "Test moteur");
Button TEST_COUL(0,245,400,100,"Test capteurs couleur");
Button TEST_BRAS(0,355,400,100,"Test Bras");
Button TEST_AUDIO(0,465,400,100,"Test Audio");
-Button TEST_ELECTRV(0,575,400,100,"Test Electro Vannes");
+Button TEST_ELECTRV(0,575,400,100,"Test Divers");
//menu demo/demo/
Button TEST_A(0,25,195,100,"A");
Button TEST_B(205,25,195,100,"B");
@@ -163,7 +166,7 @@
void print_segment(int nombre, int decalage);
void affichage_compteur (int nombre);
void effacer_segment(long couleur);
-
+unsigned char doAction(unsigned char id, unsigned short arg1, short arg2);
unsigned short telemetreDistance=0;
unsigned short telemetreDistance_avant_gauche=0;
unsigned short telemetreDistance_avant_droite=0;
@@ -210,7 +213,7 @@
{
if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
gameEtat = ETAT_GAME_START;//On débute le match
- //etat=COMPTEUR;
+ etat=COMPTEUR;
blocage_balise=1;
}
}
@@ -404,19 +407,20 @@
lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"DEMO", LEFT_MODE);
RETOUR.Draw(0xFFFF0000, 0);
TEST_VENT.Draw(VERT, 0);
- TEST_TELE.Draw(VERT, 0);
+ TEST_MOT.Draw(VERT, 0);
TEST_COUL.Draw(VERT, 0);
TEST_BRAS.Draw(VERT, 0);
TEST_AUDIO.Draw(VERT, 0);
TEST_ELECTRV.Draw(VERT, 0);
+ maximilien=0;
while (etat == DEMO) {
canProcessRx();
if(TEST_VENT.Touched()) {
while(TEST_VENT.Touched());
etat = TEST_VENTOUSE;
- } else if(TEST_TELE.Touched()) {
- while(TEST_TELE.Touched());
- etat = TEST_TELEMETRE;
+ } else if(TEST_MOT.Touched()) {
+ while(TEST_MOT.Touched());
+ etat = TEST_MOTEUR;
} else if(TEST_ELECTRV.Touched()) {
while(TEST_ELECTRV.Touched());
etat = TEST_ELECTROV;
@@ -428,8 +432,7 @@
etat =TEST_SERVO_BRAS ;
} else if(TEST_AUDIO.Touched()) {
while(TEST_AUDIO.Touched());
- TEST_AUDIO.Draw(VERT, 0);
- Debug_Audio(3,2);
+ etat=TEST_AUD;
} else if(RETOUR.Touched()) {
while(RETOUR.Touched());
etat = CHOIX;
@@ -457,29 +460,29 @@
etat=DEMO;
} else if(TEST_A.Touched()) {
while (TEST_A.Touched());
- SendRawId(TEST_VENT_A);
+ //SendRawId(TEST_VENT_A);
TEST_A.Draw(BLEU, BLANC);
} else if(TEST_NUM.Touched()) {
while (TEST_NUM.Touched());
TEST_NUM.Draw(BLEU, BLANC);
switch(maximilien){
case 1:
- SendRawId(TEST_VENT_1);
+ //SendRawId(TEST_VENT_1);
break;
case 2:
- SendRawId(TEST_VENT_2);
+ //SendRawId(TEST_VENT_2);
break;
case 3:
- SendRawId(TEST_VENT_3);
+ //SendRawId(TEST_VENT_3);
break;
case 4:
- SendRawId(TEST_VENT_4);
+ //SendRawId(TEST_VENT_4);
break;
case 5:
- SendRawId(TEST_VENT_5);
+ //SendRawId(TEST_VENT_5);
break;
case 6:
- SendRawId(TEST_VENT_6);
+ //SendRawId(TEST_VENT_6);
break;
}
} else if(TEST_PLUS.Touched()) {
@@ -507,49 +510,51 @@
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.Clear (LCD_COLOR_WHITE);
lcd.Clear (LCD_COLOR_WHITE);
- lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Electro Vannes", LEFT_MODE);
- sprintf(toto,"%hd",maximilien);
- lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
+ lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"A : Manches B : Pavillon", LEFT_MODE);
RETOUR.Draw(0xFFFF0000,0);
TEST_A.Draw(BLEU, BLANC);
- TEST_NUM.Draw(BLEU, BLANC);
- TEST_PLUS.Draw(BLEU, BLANC);
- TEST_MOINS.Draw(BLEU, BLANC);
+ TEST_B.Draw(BLEU, BLANC);
while(etat==TEST_ELECTROV) {
if(RETOUR.Touched()) {
while (RETOUR.Touched());
etat=DEMO;
} else if(TEST_A.Touched()) {
while (TEST_A.Touched());
- SendRawId(TEST_ELECTROV_A);
+ //SendRawId(TEST_ELECTROV_A);
TEST_A.Draw(BLEU, BLANC);
+ } else if(TEST_B.Touched()) {
+ while (TEST_B.Touched());
+ //SendRawId(TEST_ELECTROV_A);
+ TEST_B.Draw(BLEU, BLANC);
+ }
+ }
+ break;
+
+ case TEST_AUD:
+
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.Clear (LCD_COLOR_WHITE);
+ lcd.Clear (LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Audio", LEFT_MODE);
+ sprintf(toto,"%hd",maximilien);
+ lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE);
+ RETOUR.Draw(0xFFFF0000,0);
+ TEST_NUM.Draw(BLEU, BLANC);
+ TEST_PLUS.Draw(BLEU, BLANC);
+ TEST_MOINS.Draw(BLEU, BLANC);
+ while(etat==TEST_AUD) {
+ if(RETOUR.Touched()) {
+ while (RETOUR.Touched());
+ etat=DEMO;
} else if(TEST_NUM.Touched()) {
while (TEST_NUM.Touched());
TEST_NUM.Draw(BLEU, BLANC);
- switch(maximilien){
- case 1:
- SendRawId(TEST_ELECTROV_1);
- break;
- case 2:
- SendRawId(TEST_ELECTROV_2);
- break;
- case 3:
- SendRawId(TEST_ELECTROV_3);
- break;
- case 4:
- SendRawId(TEST_ELECTROV_4);
- break;
- case 5:
- SendRawId(TEST_ELECTROV_5);
- break;
- case 6:
- SendRawId(TEST_ELECTROV_6);
- break;
- }
+ Debug_Audio(3,maximilien);
} else if(TEST_PLUS.Touched()) {
while (TEST_PLUS.Touched());
TEST_PLUS.Draw(BLEU, BLANC);
- if(maximilien!=6)maximilien++;
+ if(maximilien!=10)maximilien++;
sprintf(toto,"%hd",maximilien);
lcd.SetBackColor(LCD_COLOR_WHITE);
lcd.SetTextColor(LCD_COLOR_BLACK);
@@ -565,7 +570,7 @@
}
}
break;
-
+
case TEST_SERVO_BRAS:
lcd.SetBackColor(LCD_COLOR_WHITE);
@@ -659,21 +664,28 @@
}
break;
- case TEST_TELEMETRE: //AFFICHAGE DE LA VALEUR LUE PAR LES 4 TELEMETRES
- ModeDemo=1;
+ case TEST_MOTEUR:
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.Clear (LCD_COLOR_WHITE);
lcd.Clear (LCD_COLOR_WHITE);
- lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Telemetre", LEFT_MODE);
- RETOUR.Draw(0xFFFF0000, 0);
- while(etat==TEST_TELEMETRE) {
- SendRawId(DATA_RECALAGE);
- SendRawId(DATA_TELEMETRE_LOGIQUE);
- wait_ms(100);
- canProcessRx();
+ lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Moteur", LEFT_MODE);
+ RETOUR.Draw(0xFFFF0000,0);
+ TEST_A.Draw(BLEU, BLANC);
+ TEST_B.Draw(BLEU, BLANC);
+ while(etat==TEST_MOTEUR) {
if(RETOUR.Touched()) {
- while( RETOUR.Touched());
+ while (RETOUR.Touched());
etat=DEMO;
- ModeDemo=0;
- }
+ } else if(TEST_A.Touched()) {
+ while (TEST_A.Touched());
+ BendRadius(150,1800,1,0);
+ TEST_A.Draw(BLEU, BLANC);
+ } else if(TEST_B.Touched()) {
+ while (TEST_B.Touched());
+ BendRadius(150,1800,0,0);
+ TEST_B.Draw(BLEU, BLANC);
+ }
}
break; ///////////////////////////////////////////FIN DES DEMOS/////////////////////////////////////////////////
@@ -964,7 +976,7 @@
gameEtat = ETAT_GAME_WAIT_FOR_JACK;
Debug_Audio(3,7);
- if (etat == TEST_TELEMETRE|| etat ==TEST_VENTOUSE || etat == TEST_COULEUR || etat ==TEST_SERVO_BRAS) {
+ if (etat == TEST_MOTEUR|| etat ==TEST_VENTOUSE || etat == TEST_COULEUR || etat ==TEST_SERVO_BRAS) {
SendRawId(DEBUG_FAKE_JAKE);
} else {
etat = AFF_WAIT_JACK;
@@ -1485,7 +1497,7 @@
int tempo = 0;
waitingAckID= ACK_ACTION; //On veut un ack de type action
waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex
- //tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
+ tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
// unsigned char test=(unsigned char) tempo;
// SendMsgCan(0x5BD, &test,1);
if(tempo == 1) {
@@ -1948,8 +1960,6 @@
}
break;
- case SOMO:
- break;
default:
break;
/*
@@ -2435,3 +2445,339 @@
return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
}
+/*************************************************************************************************/
+/* FUNCTION NAME: doAction */
+/* DESCRIPTION : Effectuer une action specifique correspondant au numéro dans le fichier strat */
+/*************************************************************************************************/
+
+unsigned char doAction(unsigned char id, unsigned short arg1, short arg2)
+{
+ int retour = 1;
+ CANMessage msgTx=CANMessage();
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ switch(id)
+ {
+ case 101: //bras avant droit
+ SendRawId(TEST_BRAS_1_AT);
+ break;
+ case 102: //bras avant millieu
+ SendRawId(TEST_BRAS_2_AT);
+ break;
+ case 103: //attraper presentoir arriere
+ //SendRawId(PRESENTOIR_ARRIERE);
+ break;
+ case 104: //balance avant
+ //SendRawId(BALANCE_AVANT);
+ break;
+ case 105: //balance arriere
+ //SendRawId(BALANCE_ARRIERE);
+ break;
+ case 106: //accelerateur avant
+ //SendRawId(ACCELERATEUR_AVANT);
+ //distance_recalage=arg1;
+ //distance_revenir=arg2;
+ //Send2Short(ACCELERATEUR_AVANT, distance_recalage,distance_revenir);
+ break;
+ case 107: //accelerateur arriere
+ //distance_recalage=arg1;
+ //distance_revenir=arg2;
+ //Send2Short(ACCELERATEUR_ARRIERE, distance_recalage,distance_revenir);
+ break;
+ case 108: //goldenium avant
+ //SendRawId(GOLDENIUM_AVANT);
+ x = arg1;
+ if(InversStrat == 1) {
+ y = 3000 - arg2;
+ } else {
+ y = arg2;
+ }
+ //Send2Short(GOLDENIUM_AVANT, x, y);
+ break;
+ case 109: //goldenium arriere
+ x=arg1;
+ y=arg2;
+ //Send2Short(GOLDENIUM_ARRIERE, x,y);
+ break;
+ case 110: //sol avant
+ //SendRawId(SOL_AVANT);
+ case 130: //sol avant
+ SendRawId(TEST_BRAS_1_RE);
+ break;
+ case 131: //sol avant
+ SendRawId(TEST_BRAS_2_RE);
+ break;
+ case 132: //sol avant
+ SendRawId(TEST_BRAS_3_RE);
+ break;
+ case 133: //sol avant
+ SendRawId(TEST_BRAS_4_RE);
+ break;
+ case 134: //sol avant
+ SendRawId(TEST_BRAS_5_RE);
+ break;
+ case 135: //sol avant
+ SendRawId(TEST_BRAS_6_RE);
+ break;
+ case 111: //sol arriere
+ // SendRawId(SOL_ARRIERE);
+ break;
+ case 112: //sol avant relache
+ //SendRawId(SOL_AVANT_RELACHE);
+ break;
+ case 113: //sol arriere relache
+ //SendRawId(SOL_ARRIERE_RELACHE);
+ break;
+ case 114://relache les 3 ventouses avants
+ // SendRawId(AVANT_RELACHE);
+ break;
+
+ case 115://bras avant gauche attraper
+ SendRawId(TEST_BRAS_3_AT);
+ break;
+
+ case 116://bras arreiere droit
+ SendRawId(TEST_BRAS_6_AT);
+ break;
+ /* case 116://relache une pompe
+ unsigned char arg_tempo;
+ if(InversStrat == 1) {
+ switch(arg1) {
+ case AV_DROIT:
+ arg_tempo = AV_GAUCHE;
+ break;
+ case AV_GAUCHE:
+ arg_tempo = AV_DROIT;
+ break;
+ case AR_DROIT:
+ arg_tempo = AR_GAUCHE;
+ break;
+ case AR_GAUCHE:
+ arg_tempo = AR_DROIT;
+ break;
+ default :
+ arg_tempo =(unsigned char)arg1;
+ break;
+ }
+
+ } else arg_tempo =(unsigned char)arg1;
+ SendMsgCan(HACHEUR_RELEASE_ATOM, &arg_tempo,1);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;*/
+ case 117://bras arriere millieu
+ SendRawId(TEST_BRAS_5_AT);
+ break;
+ case 118://bras arriere gauche
+ SendRawId(TEST_BRAS_4_AT);
+ break;
+ case 119:
+ //SendRawId(VENTOUSE_AR_CENTRE_BALANCE);
+ break;
+ case 120:
+ //SendRawId(ACCELERATEUR_INSERTION_AVANT_GAUCHE);
+ break;
+
+ case 121:
+ //SendRawId(ACCELERATEUR_INSERTION_ARRIERE_GAUCHE);
+ break;
+
+ case 150:
+ SCORE_PR+=arg1;
+ liaison_Tx.envoyer_short(0x30,SCORE_PR);
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+ break;
+
+
+ case 200 :
+ SendRawId(DATA_TELEMETRE);
+ /*telemetreDistance = dataTelemetre();
+ wait_ms(1);
+ telemetreDistance = dataTelemetre();
+ telemetreDistance = telemetreDistance - 170;*/
+ break;
+
+ case 201 :
+ SendRawId(0x99);//
+ retour = 2;
+ break;
+
+
+ case 11://0 Désactiver le stop,1 Activer le stop saut de strat,2 Activer le stop avec evitement
+ isStopEnable =(unsigned char) arg1;
+ // SendMsgCan(0x5BC, &isStopEnable,1);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+
+
+
+ case 20://Désactiver l'asservissement
+ setAsservissementEtat(0);
+ break;
+
+ case 21://Activer l'asservissement
+ setAsservissementEtat(1);
+ break;
+
+ case 22://Changer la vitesse du robot
+ SendSpeed(arg1);//,(unsigned short)arg2, (unsigned short)arg2);
+ wait_us(200);
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+ break;
+ case 23:
+ SendAccel(arg1,(unsigned short)arg2);//,(unsigned short)arg2, (unsigned short)arg2);
+ wait_us(200);
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+ break;
+
+
+ case 19: // CHANGER LA VITESSE + DECELERATION
+ //SendSpeedDecel(arg1,(unsigned short) arg2);
+ wait_us(200);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+
+ case 30://Action tempo
+ wait_ms(arg1);
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+ break;
+ //pompe 1 à 6 ON
+ case 202 :
+ //SendRawId(TEST_VENT_1_ON);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 203 :
+ //SendRawId(TEST_VENT_2_ON);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 204 :
+ //SendRawId(TEST_VENT_3_ON);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 205 :
+ // SendRawId(TEST_VENT_4_ON);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 206 :
+ // SendRawId(TEST_VENT_5_ON);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 207 :
+ // SendRawId(TEST_VENT_6_ON);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 208 :
+ // SendRawId(TEST_VENT_1_OFF);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 209 :
+ //SendRawId(TEST_VENT_2_OFF);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 210 :
+ // SendRawId(TEST_VENT_3_OFF);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 211 :
+ //SendRawId(TEST_VENT_4_OFF);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 212 :
+ // SendRawId(TEST_VENT_5_OFF);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 213 :
+ //SendRawId(TEST_VENT_6_OFF);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+ case 214 :
+ SendRawId(BRAS_AT);
+ break;
+
+ case 215://test bras competition ( à tester)
+ unsigned char arg_tempo;
+ switch(arg1)
+ {
+ case AV_DROIT:
+ arg_tempo = 1;
+ break;
+ case AV_CENTRE:
+ arg_tempo = 2;
+ break;
+ case AV_GAUCHE:
+ arg_tempo = 3;
+ break;
+ case AR_DROIT:
+ arg_tempo = 4;
+ break;
+ case AR_CENTRE:
+ arg_tempo = 5;
+ break;
+ case AR_GAUCHE:
+ arg_tempo = 6;
+ break;
+ default :
+ arg_tempo =(unsigned char)arg1;
+ break;
+ }
+ SendMsgCan(BRAS_AT, 0,arg_tempo);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+
+ case 216://test bras competition ( à tester)
+ unsigned char arg_tempo1;
+ switch(arg1)
+ {
+ case AV_DROIT:
+ arg_tempo1 = 1;
+ break;
+ case AV_CENTRE:
+ arg_tempo1 = 2;
+ break;
+ case AV_GAUCHE:
+ arg_tempo1 = 3;
+ break;
+ case AR_DROIT:
+ arg_tempo1 = 4;
+ break;
+ case AR_CENTRE:
+ arg_tempo1 = 5;
+ break;
+ case AR_GAUCHE:
+ arg_tempo1 = 6;
+ break;
+ default :
+ arg_tempo1 =(unsigned char)arg1;
+ break;
+ }
+ //SendMsgCan(BRAS_RE, 0,arg_temp1);
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+
+ default:
+ retour = 0;//L'action n'existe pas, il faut utiliser le CAN
+
+ }
+ return retour;//L'action est spécifique.
+
+}
--- a/main.cpp Wed Mar 04 12:38:22 2020 +0000
+++ b/main.cpp Wed Mar 11 14:21:02 2020 +0000
@@ -69,7 +69,7 @@
lcd.DisplayStringAt(0, 0,(uint8_t *)"Initialisation", LEFT_MODE);
led1 = 1;
bluetooth_init();
- //lecture_fichier(); //bloquant si pas de carte SD
+ lecture_fichier(); //bloquant si pas de carte SD
led1 = 0;
wait_ms(2000);//Attente pour que toutes les cartes se lancent et surtout le CANBlue
while(true) {