CRAC Team
code de la carte IHM avant les bugs et avant le travail effectué avec Melchior
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Diff: Strategie/Strategie.cpp
- Revision:
- 14:6aa8aa1699ad
- Parent:
- 11:848ea39838bf
- Child:
- 15:5ece9c3ba389
--- 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.
+
+}