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 1:7e925468f9d9, committed 2020-01-30
- Comitter:
- gabrieltetar
- Date:
- Thu Jan 30 16:48:59 2020 +0000
- Parent:
- 0:41cc45429aba
- Child:
- 2:8fcdc11bd693
- Commit message:
- start
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Conv_data_CAN/conv_data.cpp Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,19 @@
+#include "global.h"
+
+short char_to_short_transformation(char input1, char input2)
+{
+ short output = 0;
+
+ output = input2<<8;
+ output += input1;
+
+ return output;
+}
+
+void short_to_char_transformation(char* output1, char* output2, short input)
+{
+ *output1 = input & 0x00FF;
+
+ *output2 = (input & 0xFF00)>>8;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Conv_data_CAN/conv_data.h Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,2 @@ +short char_to_short_transformation(char input1, char input2); +void short_to_char_transformation(char* output1, char* output2, short input); \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Evitement/Evitement.cpp Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,136 @@
+/*********************************************\
+| Dodge algorithm |
+| for CRAC Team 2020 |
+| by Gabriel Tetar |
+\*********************************************/
+
+// TABLE
+ /*******************************************************\----> Y 0;3000
+ | | | |
+ | JAUNE | | VIOLET |
+ | | | |
+ | | | |
+ | |-------------------------------------| | X 0;2000
+ | | | |
+ | | | |
+ | | | |
+ | | | |
+ O*******************************************************/
+
+
+#include "global.h"
+#include <math.h>
+#define M_PI 3.14159265358979323846f
+/****************************************************************************************/
+/* FUNCTION NAME: Balise Danger */
+/* DESCRIPTION : FIFO -> BALISE_DANGER */
+/****************************************************************************************/
+unsigned short balise_danger(void){
+ SendSpeed(150);
+ return(0);
+}
+/****************************************************************************************/
+/* FUNCTION NAME: Balise Stop */
+/* DESCRIPTION : FIFO -> BALISE_STOP */
+/****************************************************************************************/
+unsigned short balise_stop(signed char FIFO_lecture){
+
+ signed char fin_angle_detection;
+ signed char debut_angle_detection;
+ float angle_moyen_balise_IR = 0.0;
+
+ //on recupere l'info d'angle de detection--------------------------------------
+ if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
+ fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
+ debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
+ } else { //data balise Gros Robot Detecte
+ fin_angle_detection = msgRxBuffer[FIFO_lecture].data[2] & 0x0F;
+ debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[2] & 0xF0) >> 4;
+ }
+ //on moyenne l'angle------------------------------------------------------------
+ if(debut_angle_detection > fin_angle_detection) {
+ angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
+ if(angle_moyen_balise_IR > 15.0f)
+ angle_moyen_balise_IR-=15.0f;
+ } else
+ angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
+ #ifdef ROBOT_BIG
+ float seuil_bas_avant = 12.0;
+ float seuil_haut_avant = 15.0;
+ float seuil_bas_arriere = 5.0;
+ float seuil_haut_arriere = 7.0;
+ #else
+ float seuil_bas_arriere = 12.0;
+ float seuil_haut_arriere = 15.0;
+ float seuil_bas_avant = 4.0;
+ float seuil_haut_avant = 7.0;
+ #endif
+ if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere))
+ SendRawId(ASSERVISSEMENT_STOP);
+ return(0);
+}
+/****************************************************************************************/
+/* FUNCTION NAME: Balise end Danger */
+/* DESCRIPTION : FIFO -> BALISE_END_DANGER */
+/****************************************************************************************/
+unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, E_stratGameEtat* gameEtat, signed short target_x_robot, signed short target_y_robot, signed short target_theta_robot, signed short theta_robot,signed short x_robot,signed short y_robot){
+ switch(instruction->order){
+ case MV_LINE:
+ *gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ instruction->order = MV_XYT;
+ instruction->arg1 = target_x_robot;// X
+ instruction->arg2 = target_y_robot;// Y
+ instruction->arg3 = target_theta_robot;// T
+ break;
+ case MV_TURN:
+ *gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ instruction->order = MV_XYT;
+ instruction->arg1 = target_x_robot;// X
+ instruction->arg2 = target_y_robot;// Y
+ instruction->arg3 = target_theta_robot;// T
+ break;
+ case MV_XYT:
+ *gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ break;
+ case MV_COURBURE:
+ unsigned short alpha;
+ *gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ instruction->order=MV_XYT;
+ if(instruction->direction==LEFT) alpha=(dodgeq->inst[0].arg3-theta_robot);
+ else alpha=(theta_robot-dodgeq->inst[0].arg3);
+ if(alpha<450){
+ dodgeq->nb=0;
+ instruction->arg1=dodgeq->inst[0].arg1;//x
+ instruction->arg2=dodgeq->inst[0].arg2;//y
+ instruction->arg3=dodgeq->inst[0].arg3;//t
+ } else if(alpha<900){
+ dodgeq->nb=1;
+ instruction->arg1=dodgeq->inst[1].arg1;//x
+ instruction->arg2=dodgeq->inst[1].arg2;//y
+ instruction->arg3=dodgeq->inst[1].arg3;//t
+ } else if(alpha<1350){
+ dodgeq->nb=2;
+ instruction->arg1=dodgeq->inst[2].arg1;//x
+ instruction->arg2=dodgeq->inst[2].arg2;//y
+ instruction->arg3=dodgeq->inst[2].arg3;//t
+ } else if(alpha<1800){
+ dodgeq->nb=3;
+ instruction->arg1=dodgeq->inst[3].arg1;//x
+ instruction->arg2=dodgeq->inst[3].arg2;//y
+ instruction->arg3=dodgeq->inst[3].arg3;//t
+ } else if(alpha<2250){
+ dodgeq->nb=4;
+ instruction->arg1=dodgeq->inst[4].arg1;//x
+ instruction->arg2=dodgeq->inst[4].arg2;//y
+ instruction->arg3=dodgeq->inst[4].arg3;//t
+ } else {
+ dodgeq->nb=5;
+ instruction->arg1=dodgeq->inst[5].arg1;//x
+ instruction->arg2=dodgeq->inst[5].arg2;//y
+ instruction->arg3=dodgeq->inst[5].arg3;//t
+ }
+ break;
+ }
+ SendSpeed(300);
+ return(0);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Evitement/Evitement.h Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,21 @@ +/*********************************************\ +| Dodge algorithm | +| for CRAC Team 2020 | +| by Gabriel Tetar | +\*********************************************/ +#ifndef CRAC_EVITEMENT +#define CRAC_EVITEMENT + +#define T_X 2000 +#define T_Y 3000 + +unsigned short balise_danger(void); +unsigned short balise_stop(signed char FIFO_lecture); +unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, E_stratGameEtat* gameEtat, signed short target_x_robot, signed short target_y_robot, signed short target_theta_robot, signed short theta_robot,signed short x_robot,signed short y_robot); + + + + + +#endif +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Globals/constantes.h Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,60 @@ +#ifndef CRAC_CONSTANTES +#define CRAC_CONSTANTES +// **************************************************************************************** +// * CONSTANTES SYMBOLIQUES * +// **************************************************************************************** +#define ESPACE_INTER_TELEMETRE 149.98//PR 2019 + + +#define SIZE_FIFO 50 //Taille du buffer pour le bus CAN + +#define SIZE 750 //Taille d'une ligne du fichier +#define SIZE_BUFFER_FILE 150 //Taille du buffer d'instruction + + +/**** +** Variable à modifier en fonction du robot +***/ + +//------------------------------------sellection Robot--------------------- +// +#define ROBOT_BIG //Si commenté Petit Robot, si Décommenter Gros Robot + +#ifndef ROBOT_BIG // !!!!!!!! ne pas commenter + #define ROBOT_SMALL // !!!!!!!! ne pas commenter +#endif // !!!!!!!! ne pas commenter + +//------------------------------------------------------------------------- + + +#ifdef ROBOT_BIG + + #define NOMBRE_CARTES 2//Le nombre de carte présente sur le gros robot + //#define POSITION_DEBUT_X 1830 + //#define POSITION_DEBUT_Y 900 + //#define POSITION_DEBUT_T 180 + + #define POSITION_DEBUT_X 200 + #define POSITION_DEBUT_Y 880 + #define POSITION_DEBUT_T 0 + #define MOITIEE_ROBOT 61 + //#define POSITION_DEBUT_X 0 + //#define POSITION_DEBUT_Y 0 + //#define POSITION_DEBUT_T 0 + + #define BALISE_TIMEOUT 5000 + +#else + + #define NOMBRE_CARTES 2//Le nombre de carte présente sur le petit robot + #define POSITION_DEBUT_X 210 + #define POSITION_DEBUT_Y 285 + #define POSITION_DEBUT_T 0 + #define MOITIEE_ROBOT 90 + #define BALISE_TIMEOUT 2000 + +#endif + + + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Globals/global.h Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,69 @@ +#ifndef GLOBAL_H +#define GLOBAL_H + +#include "mbed.h" +#include "ident_crac.h" + + +#include <CAN.h> +#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" +#include "TS_DISCO_F469NI.h" +#include "LCD_DISCO_F469NI.h" +#include "fonts.h" +#include "F469_GUI.hpp" +#include "Config_big.h" +#include "LiaisonBluetooth.h" +#include "Evitement.h" + +//#include "peripheriques.h" +extern Serial pc; +extern Serial rn42_Tx; +extern Serial rn42_Rx; +extern Serial rn42_pr; +extern LiaisonBluetooth liaison_Tx; +extern LiaisonBluetooth liaison_Rx; +extern LiaisonBluetooth liaison_pr; + +extern CAN can1; +extern CAN can2; +extern CANMessage msgRxBuffer[SIZE_FIFO]; +extern unsigned char FIFO_ecriture; +extern char strat_sd[10][SIZE+8]; +extern char PATH[10][SIZE+8]; + +extern signed char Strategie; + +extern int ack_bluetooth; + +extern DigitalOut led1,led2,led3,led4; +extern LCD_DISCO_F469NI lcd; +extern char cheminFileStart[SIZE+8]; //Le chemin du fichier de strat, utiliser strcpy(cheminFileStart,"/local/strat.txt"); +extern short SCORE_GLOBAL; +extern short SCORE_PR, SCORE_GR; + +extern struct S_Instruction strat_instructions[SIZE_BUFFER_FILE]; //La liste des instruction chargé en mémoire +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 char couleur1, couleur2, couleur3; +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 +extern unsigned short waitingAckFrom;//La provenance du ack attendu +extern char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre +extern unsigned short waitingId; + +extern unsigned char isStopEnable; +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Globals/ident_crac.lib Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#f0b20feb32ba
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Globals/ident_crac_2.h Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,233 @@ +#ifndef CRAC_IDENTH +#define CRAC_IDENTH + +#define GLOBAL_GAME_END 0x004 // Stop fin du match +#define GLOBAL_START 0x002 // Start +#define GLOBAL_END_INIT_POSITION 0x005 // Fin positionnement robot avant depart +#define GLOBAL_FUNNY_ACTION 0x007 // Funny action start (0: start, 1: stop) +#define GLOBAL_JACK 0x008 +#define ACKNOWLEDGE_JACK 0X009 + +//#define BALISE_STOP 0x003 // Trame stop + +#define BALISE_DANGER 0xA // Trame danger + +#define BALISE_END_DANGER 0xB // Trame fin de danger + + +#define ASSERVISSEMENT_STOP 0x001 // Stop moteur +#define ASSERVISSEMENT_SPEED_DANGER 0x006 // Vitesse de danger + +#define ASSERVISSEMENT_XYT 0x020 // Asservissement (x,y,theta) (0 : au choix 1 : avant -1 : arrière) +#define ASSERVISSEMENT_XYT_ROTATE 0x030//premiere rotation durant xy theta +#define ASSERVISSEMENT_XYT_LINE 0x040//ligne droite durant xy theta + + +#define ASSERVISSEMENT_COURBURE 0x021 // Asservissement rayon de courbure (+ gauche, - droite , sens : 1avt , -1arr; enchainement => 1 oui, 0 => non, 2=>derniére instruction de l'enchainement) +#define ASSERVISSEMENT_CONFIG 0x022 // Asservissement paramètre (définir les valeurs de vitesse max et d'eccélération max) +#define ASSERVISSEMENT_ROTATION 0x023 // Asservissement rotation +#define ASSERVISSEMENT_RECALAGE 0x024 // Moteur tout droit (recalage : 0 mouvement seul, 1 x, 2y valeur : coordonnée à laquelle est recalé x/y; enchainement => 1 oui, 0 => non) +#define ACTION_BIG_DEMARRAGE 0x025 // Action de départ du GR (Lancement de la trajectoire de départ du GR) +#define ODOMETRIE_BIG_POSITION 0x026 // Odométrie position robot (Position actuel du robot) +#define ODOMETRIE_BIG_VITESSE 0x027 // Odométrie vitesse (Indication sur l'état actuel) +#define ODOMETRIE_SMALL_POSITION 0x028 // Odométrie position robot (Position actuel du robot) +#define ODOMETRIE_SMALL_VITESSE 0x029 // Odométrie vitesse (Indication sur l'état actuel) + + + +#define ASSERVISSEMENT_CONFIG_DECEL 0x019 // Asservissement paramètre (définir les valeurs de vitesse max et de decélération max) + + +//////////////////////////////////////////////////////////RESETS/////////////////////////////////////////////////// +#define RESET_BALISE 0x030 // Reset balise +#define RESET_MOTEUR 0x031 // Reset moteur +#define RESET_IHM 0x032 // Reset écran tactile +#define RESET_ACTIONNEURS 0x033 // Reset actionneurs +#define RESET_POMPES 0x034 // Reset pompes +#define RESET_AX12 0x035 // Reset AX12 +#define RESET_TELEMETRE 0x036 // Reset telemetre + + + +#define RESET_STRAT 0x3A // Reset stratégie + +//////////////////////////////////////////////////////////CHECK CARTES///////////////////////////////////////////////// +#define CHECK_BALISE 0x060 // Check balise +#define CHECK_MOTEUR 0x061 // Check moteur +#define CHECK_IHM 0x062 // Check écran tactile +#define CHECK_ACTIONNEURS_AVANT 0x063 // Check actionneurs +#define CHECK_ACTIONNEURS_ARRIERE 0x064 // Check pompes +#define CHECK_AX12 0x065 // Check AX12 +#define CHECK_OK_TELEMETRE 0x066 // Check telemetre + +//////////////////////////////////////////////////////////ACK CARTES/////////////////////////////////////////////////// +#define ALIVE_BALISE 0x070 // Alive balise +#define ALIVE_MOTEUR 0x071 // Alive moteur +#define ALIVE_IHM 0x072 // Alive écran tactile +#define ALIVE_ACTIONNEURS_AVANT 0x073 // Alive actionneurs +#define ALIVE_ACTIONNEURS_ARRIERE 0x074 // Alive pompes +#define ALIVE_HERKULEX 0x075 // Alive AX12 +#define ALIVE_TELEMETRE 0x076 // Alive telemetre + + +/////////////////////////////////////////////////////ACTIONS COMPLEXES///////////////////////////////////////////////// +#define MONTER_IMMEUBLE_DOUBLE 0x090 // Monte deux immeubles selon un code couleur +#define MONTER_IMMEUBLE 0x091 +#define ACK_ACTION 0x99 //autre action possible via les herkulex, ne peut pas passer en sendrawid + + + +/////////////////////////////////////////////////////////ACKS//////////////////////////////////////////////////////////// +#define ACKNOWLEDGE_BALISE 0x100 // Acknowledge balise +#define ACKNOWLEDGE_MOTEUR 0x101 // Acknowledge moteur +#define ACKNOWLEDGE_IHM 0x102 // Acknowledge ecran tactile +#define ACKNOWLEDGE_ACTIONNEURS 0x103 // Acknowledge actionneurs +#define ACKNOWLEDGE_POMPES 0x104 // Acknowledge pompes +#define ACKNOWLEDGE_TELEMETRE 0x105 // Acknowledge telemetre +#define ACKNOWLEDGE_HERKULEX 0x106 // Ack ax12 +#define ACKNOWLEDGE_STRAT 0x10A // Acknowledge pompes +#define ACKNOWLEDGE_CAMERA 0x108 //Acknowledge couleur caméra + + +#define INSTRUCTION_END_BALISE 0x110 // Fin instruction balise (Indique que l'instruction est terminée) +#define INSTRUCTION_END_MOTEUR 0x111 // Fin instruction moteur (Indique que l'instruction est terminée) +#define INSTRUCTION_END_IHM 0x112 // Fin instruction ecran tactile (Indique que l'instruction est terminée) +#define INSTRUCTION_END_ACTIONNEURS 0x113 // Fin instruction actionneurs (Indique que l'instruction est terminée) +#define ACK_FIN_ACTION 0x116 + + + +/////////////////////////////////////////////////////////ERREURS//////////////////////////////////////////////////////// +#define ERROR_OVERFLOW_BALISE 0x040 // Overflow odométrie +#define ERROR_OVERFLOW_MOTEUR 0x041 // Overflow asservissement +#define ERROR_OVERFLOW_IHM 0x042 // Overflow balise +#define ERROR_OVERFLOW_STRAT 0x043 // Overflow stratégie +#define ERROR_BALISE 0x785 // Bug balise +#define ERROR_RTC 0x786 // Bug RTC +#define ERROR_MOTEUR 0x787 // Bug moteur +#define ERROR_TELEMETRIE 0x788 // Bug télémètre +#define ERROR_STRATEGIE 0x789 // Bug stratégie + + + +/////////////////////////////////////////////////ACTIONS SIMPLES DU ROBOT///////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//#define BAISSER_ATTRAPE_BLOC 0x220 +#define RELEVER_ATTRAPE_BLOC 0x221 + +#define GABARIT_PETIT_ROBOT 0x200 + +#define PRESENTOIR_AVANT 0x201 +#define PRESENTOIR_ARRIERE 0x202 + +#define BALANCE_AVANT 0x203 +#define BALANCE_ARRIERE 0x204 + +#define ACCELERATEUR_AVANT 0x205 +#define ACCELERATEUR_ARRIERE 0x206 + +#define GOLDENIUM_AVANT 0x207 +#define GOLDENIUM_ARRIERE 0x208 + +#define SOL_AVANT 0x209 +#define SOL_ARRIERE 0x210 + +#define SOL_AVANT_RELACHE 0x211 +#define SOL_ARRIERE_RELACHE 0x212 + +#define AVANT_RELACHE 0x213 +#define ARRIERE_RELACHE 0x214 + +#define RECROQUEVILLER 0x215 + +#define VENTOUSE_AV_CENTRE_BALANCE 0x216 +#define VENTOUSE_AR_CENTRE_BALANCE 0x217 + +#define ACCELERATEUR_INSERTION_AVANT_GAUCHE 0X218 +#define ACCELERATEUR_INSERTION_DERRIERE_GAUCHE 0X219 + +//commande pompe +#define HACHEUR_GET_ATOM 0x520 +#define HACHEUR_GET_ATOM_ACK 0x521 + +#define HACHEUR_RELEASE_ATOM 0x522 +#define HACHEUR_RELEASE_ATOM_ACK 0x523 + +#define HACHEUR_GET_PRESENTOIR_AV 0x524 +#define HACHEUR_RELEASE_AV 0x525 + +#define HACHEUR_GET_PRESENTOIR_AR 0x526 +#define HACHEUR_RELEASE_AR 0x527 + +#define HACHEUR_STATUT_VENTOUSES 0x528 + +//#define BAISSER_ATTRAPE_BLOC_AvG 0x222 +#define RELEVER_ATTRAPE_BLOC_AvG 0x223 + +#define BAISSER_ATTRAPE_BLOC_AvD 0x224 +#define RELEVER_ATTRAPE_BLOC_AvD 0x225 + +#define INTER_BAISSER_ATTRAPE_BLOC_AvG 0x226 +#define INTER_BAISSER_ATTRAPE_BLOC_AvD 0x227 + +#define RANGER_ATTRAPE_BLOC_AvD 0x228 +#define RANGER_ATTRAPE_BLOC_AvG 0x229 + +#define BRAS_VENTOUSE_1 0x22A +#define BRAS_VENTOUSE_2 0x22B + +#define BRAS_ABEILLE_UP 0x202 +#define BRAS_ABEILLE_DOWN 0x203 +#define INCLINAISON_CHATEAU 0x204 +#define INCLINAISON_EPURATION 0x205 +#define ALLUMER_PANNEAU_UP 0x206 +#define ALLUMER_PANNEAU_DOWN 0x207 +#define BLOCAGE_BALLE 0x208 +#define LANCEMENT_MOTEUR_TIR_ON 0x209 +#define LANCEMENT_MOTEUR_TIR_OFF 0x20A +#define AIGUILLEUR_CENTRE 0x20B +#define AIGUILLEUR_GAUCHE 0x20C +#define AIGUILLEUR_DROITE 0x20D +#define TRI_BALLE 0x20E +#define NO_BLOC 0x20F + +///////////////////////////////////////////CAPTEURS/////////////////////////////////////////////////////////////////// +#define DATA_TELEMETRE 0x310 // Demande sa valeur à un télémètre parmis les +#define RECEPTION_DATA 0x311 // envoi de la valeur d'un des télémètres +#define TELEMETRE_OBJET 0x312 +#define OBJET_SUR_TABLE 0x313 +#define RECEPTION_RECALAGE 0x315 //Valeur des télémètres +#define DATA_RECALAGE 0x316 //Demande de la valeur de tous les télémètres afin de procèder au récalage +#define LIRE_PANNEAU 0x317 +#define VIBRO 0x318 + + +#define DATA_TELEMETRE_LOGIQUE 0x319 +#define RECEPTION_TELEMETRE_LOGIQUE 0x320 +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +///////////////////////////////////////////////////ENVOI DE PARAMETRES////////////////////////////////////////////////// +#define CHOICE_COLOR 0x602 // Couleur (0->VERT;1->ORANGE) +#define RECEPTION_COULEUR 0x603 //Code Couleur +#define ECRAN_ALL_CHECK 0x620 // Carte all check (Si provient de carte strat => toutes les cartes sont en ligne, Si provient IHM => forcer le lancement) + +///////////////////////////////////////////////////////////DEBUGS/////////////////////////////////////////////////////// +#define DEBUG_STRATEGIE_AUTOMATE 0x760 // Etat automate stratégie (Permet de savoir l'etat de l'automate) +#define DEBUG_FAKE_JAKE 0x761 // Fake jack (Permet d'outre passerr le JACk du robot) +#define DEBUG_ASSERV 0x762 // Info debug carte moteur + + + +#define POMPE_PWM 0x9A // pwm des pompes (pwm entre 0 et 100) + + +#define ASSERVISSEMENT_INFO_CONSIGNE 0x1F0 // Info Consigne et Commande moteur +#define ASSERVISSEMENT_CONFIG_KPP_DROITE 0x1F1 // Config coef KPP_Droit +#define ASSERVISSEMENT_CONFIG_KPI_DROITE 0x1F2 // Config coef KPI_Droit +#define ASSERVISSEMENT_CONFIG_KPD_DROITE 0x1F3 // Config coef KPD_Droit +#define ASSERVISSEMENT_CONFIG_KPP_GAUCHE 0x1F4 // Config coef KPP_Gauche +#define ASSERVISSEMENT_CONFIG_KPI_GAUCHE 0x1F5 // Config coef KPI_Gauche +#define ASSERVISSEMENT_CONFIG_KPD_GAUCHE 0x1F6 // Config coef KPD_Gauche +#define ASSERVISSEMENT_ENABLE 0x1F7 // Activation asservissement (0 : désactivation, 1 : activation) + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Instruction/Instruction.cpp Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,175 @@
+#include "global.h"
+
+
+
+
+
+
+SDFileSystem sd(PB_15, PB_14,PD_3,PH_6,"sd");
+
+enum E_InstructionType charToInstructionType(char type)
+{
+ switch(type)
+ {
+ case 'B': return MV_BEZIER;
+ case 'C': return MV_COURBURE;
+ case 'L': return MV_LINE;
+ case 'T': return MV_TURN;
+ case 'X': return MV_XYT;
+ case 'R': return MV_RECALAGE;
+ case 'A': return ACTION;
+ case 'P': return POSITION_DEBUT;
+ default: return UNKNOWN;
+ }
+}
+
+enum E_InstructionDirection charToInstructionDirection(char type)
+{
+ switch(type)
+ {
+ case 'B': return BACKWARD;
+ case 'F': return FORWARD;
+ case 'R': return RELATIVE;
+ case 'A': return ABSOLUTE;
+ case 'L': return LEFT;
+ default: return NODIRECTION;
+ }
+}
+
+enum E_InstructionPrecisionOuRecalage charToInstructionPrecisionOuRecalage(char type)
+{
+ switch(type)
+ {
+ case 'P': return PRECISION;
+ case 'X': return RECALAGE_X;
+ case 'Y': return RECALAGE_Y;
+ case 'T': return RECALAGE_T;
+ default: return NOPRECISION;
+ }
+}
+
+enum E_InstructionNextActionType charToInstructionNextActionType(char type)
+{
+ switch(type)
+ {
+ case 'J': return JUMP;
+ case 'W': return WAIT;
+ case 'E': return ENCHAINEMENT;
+ case 'M': return MECANIQUE;
+ case 'C': return CAPTEUR;
+ default: return NONEXTACTION;
+ }
+}
+
+enum E_InstructionNextActionJumpType charToInstructionNextActionJumpType(char type)
+{
+ switch(type)
+ {
+ case 'T': return JUMP_TIME;
+ case 'P': return JUMP_POSITION;
+ default: return NONEXTACTIONJUMPTYPE;
+ }
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: stringToInstruction */
+/* DESCRIPTION : Conversion d'une ligne du fichier de strat en instruction */
+/****************************************************************************************/
+struct S_Instruction stringToInstruction(char line[]) {
+ struct S_Instruction instruction;
+
+ char instructionOrder;
+ char instructionDirection;
+ char instructionPrecision;
+ char instructionNextActionType;
+ char instructionJumpAction;
+ int errorCode = 0;
+ /*
+ Info sur la fonction sscanf
+ %d -> Entier signé
+ %u -> Entié non signé
+ %c -> char
+ */
+ errorCode = sscanf(line, "%hd,%c,%c,%hu,%hu,%hd,%c,%c,%c,%hu,%hu,%hd,%hd,%hu,%hu,%hd",
+ &instruction.lineNumber,
+ &instructionOrder,
+ &instructionDirection,
+ &instruction.arg1,
+ &instruction.arg2,
+ &instruction.arg3,
+ &instructionPrecision,
+ &instructionNextActionType,
+ &instructionJumpAction,
+ &instruction.JumpTimeOrX,
+ &instruction.JumpY,
+ &instruction.nextLineOK,
+ &instruction.nextLineError,
+ &instruction.arg4,
+ &instruction.arg5,
+ &instruction.arg6
+ );
+ /*
+ if(errorCode != 13) {
+ errorInstructionLoop();//L'instruction est pas bonne !!
+ }*/
+
+ instruction.order = charToInstructionType(instructionOrder);
+ instruction.direction = charToInstructionDirection(instructionDirection);
+ instruction.precision = charToInstructionPrecisionOuRecalage(instructionPrecision);
+ instruction.nextActionType = charToInstructionNextActionType(instructionNextActionType);
+ instruction.jumpAction = charToInstructionNextActionJumpType(instructionJumpAction);
+
+
+ return instruction;
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: loadAllInstruction */
+/* DESCRIPTION : Charger toutes les instructions du fichier de stratégie */
+/* Il faut utiliser strcpy(cheminFileStart,"/local/strat.txt"); */
+/* pour indiquer le fichier à utiliser */
+/****************************************************************************************/
+void loadAllInstruction( signed char Strategie) {
+
+ struct S_Instruction instruction;
+ char LineBuffer[SIZE];
+ printf("Reading file : ");
+ printf(strat_sd[Strategie]);
+ printf("\n");
+ strcpy(PATH[Strategie],"/sd/");
+ strcat(PATH[Strategie],strat_sd[Strategie]);
+ strcat(PATH[Strategie],".txt");
+ FILE *testFile = fopen(PATH[Strategie], "rt"); //Ouverture du fichier en mode lecture seul au format string
+
+ nb_instructions = 0;
+ while (fgets(LineBuffer, SIZE, testFile) != NULL) {
+ instruction = stringToInstruction(LineBuffer);
+ strat_instructions[nb_instructions] = instruction;
+ if(strat_instructions[nb_instructions].order == UNKNOWN) {
+ Button STRAT_1 (0, 30, 190, 110, PATH[Strategie]);
+ STRAT_1.Draw(0xFFF0F0F0, 0);
+ errorInstructionLoop();//L'instruction est pas bonne !!
+ }
+ //printf(LineBuffer);
+ //debug_Instruction(instruction);
+ nb_instructions++;
+ }
+ printf("nb instruction = %d\n",nb_instructions);
+ fclose(testFile);
+
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: FileExists */
+/* DESCRIPTION : Permet de vérifier si un fichier existe */
+/****************************************************************************************/
+int FileExists(const char *fname)
+{
+ FILE *file;
+ if (file == fopen(fname, "r"))
+ {
+ fclose(file);
+ return 1;
+ }
+ return 0;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Instruction/Instruction.h Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,126 @@
+#ifndef CRAC_INSTRUCTION
+#define CRAC_INSTRUCTION
+
+#define caca 40
+
+enum E_InstructionType
+{
+ MV_BEZIER,
+ MV_COURBURE, // C -> Courbure
+ MV_LINE, // L -> Ligne droite
+ MV_TURN, // T -> Rotation sur place
+ MV_XYT, // X -> Aller à
+ MV_RECALAGE, // R -> Recalage bordure
+ ACTION, // A -> Action
+ UNKNOWN, // Erreur, instruction inconnue
+ POSITION_DEBUT // P
+};
+enum E_InstructionDirection
+{
+ NODIRECTION, // N -> Parametre absent
+ BACKWARD,
+ FORWARD,
+ RELATIVE,
+ ABSOLUTE,
+ LEFT,
+ RIGHT
+};
+enum E_InstructionPrecisionOuRecalage
+{
+ NOPRECISION,// N -> Parametre absent
+ PRECISION, // P -> Precision, verifier la position à la fin du mouvement et refaire un XYT si erreur > 1cm
+ RECALAGE_X, // X -> Recalage en X, indique un recalage sur l'axe X
+ RECALAGE_Y, // Y -> Recalage en Y, indique un recalage sur l'axe Y
+ RECALAGE_T
+};
+enum E_InstructionNextActionType
+{
+ NONEXTACTION, // N -> Parametre absent
+ JUMP,
+ WAIT,
+ ENCHAINEMENT,
+ MECANIQUE,
+ CAPTEUR
+};
+enum E_InstructionNextActionJumpType
+{
+ NONEXTACTIONJUMPTYPE, // N -> Parametre absent
+ JUMP_TIME,
+ JUMP_POSITION
+};
+struct S_Instruction
+{
+ short lineNumber;//Numéro de la ligne
+ enum E_InstructionType order; //Type de l'instruction
+ enum E_InstructionDirection direction; //BackWard ou Forward || Relative ou Absolu
+
+ unsigned short arg1;
+ unsigned short arg2;
+ signed short arg3;
+
+ enum E_InstructionPrecisionOuRecalage precision;
+ enum E_InstructionNextActionType nextActionType;
+ enum E_InstructionNextActionJumpType jumpAction;
+ unsigned short JumpTimeOrX;
+ unsigned short JumpY;
+ unsigned short nextLineOK;
+ unsigned short nextLineError;
+
+ unsigned short arg4;
+ unsigned short arg5;
+ signed short arg6;
+};
+
+struct S_Dodge_queue
+{
+ unsigned short nb;//Nombre d action en file dattente [0,9]
+ struct S_Instruction inst[10];
+};
+
+/**
+* Convertir un char en type d'instruction
+**/
+enum E_InstructionType charToInstructionType(char type);
+
+/**
+*
+**/
+enum E_InstructionDirection charToInstructionDirection(char type);
+
+/**
+* Convertir un char
+**/
+enum E_InstructionPrecisionOuRecalage charToInstructionPrecisionOuRecalage(char type);
+
+/**
+*
+**/
+enum E_InstructionNextActionType charToInstructionNextActionType(char type);
+
+/**
+*
+**/
+enum E_InstructionNextActionJumpType charToInstructionNextActionJumpType(char type);
+
+/****************************************************************************************/
+/* FUNCTION NAME: stringToInstruction */
+/* DESCRIPTION : Conversion d'une ligne du fichier de strat en instruction */
+/****************************************************************************************/
+struct S_Instruction stringToInstruction(char line[]);
+
+/****************************************************************************************/
+/* FUNCTION NAME: loadAllInstruction */
+/* DESCRIPTION : Charger toutes les instructions du fichier de stratégie */
+/* Il faut utiliser strcpy(cheminFileStart,"/local/strat.txt"); */
+/* pour indiquer le fichier à utiliser */
+/****************************************************************************************/
+void loadAllInstruction( signed char Strategie);
+
+/****************************************************************************************/
+/* FUNCTION NAME: FileExists */
+/* DESCRIPTION : Permet de vérifier si un fichier existe */
+/****************************************************************************************/
+int FileExists(const char *fname);
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Instruction/lecture_repertoire.cpp Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,38 @@
+#include "global.h"
+
+
+struct dirent* files_name[10];
+char *rest;
+
+
+void lecture_fichier(void){
+ short i,j;
+
+ DIR* rep = NULL;
+
+ mkdir("/sd", 0777);
+ strcpy(cheminFileStart,"/sd");
+ rep=opendir("/sd");
+
+ for(i=0;i<20;i++){
+ files_name[i]= readdir(rep);
+
+ if(files_name[i]->d_name==""){
+ i=20;
+ }
+ else{
+ strcpy(strat_sd[i-j],files_name[i]->d_name);
+ rest=strstr(strat_sd[i-j],".txt");
+
+ if(rest==NULL){
+ strcpy(strat_sd[i-j],"");
+ j++;
+ }
+ else{
+ strcpy(strat_sd[i-j],strtok(strat_sd[i-j],"."));
+ }
+
+ }
+ }
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Instruction/lecture_repertoire.h Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,3 @@ + + +void lecture_fichier(void); \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Robots/Config_big.h Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,61 @@ +#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Robots/StrategieManager.h Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,55 @@ +#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Robots/Strategie_big.cpp Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,305 @@
+#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Robots/Strategie_small.cpp Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,235 @@
+#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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CRAC-Team/code/SDFileSystem/#2cf33501d9c5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SerialHalfDuplex.lib Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/CRAC-Team/code/SerialHalfDuplex/#8fd1fa67565e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Strategie/DISCO-F469NI_portrait.lib Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/CRAC-Team/code/DISCO-F469NI_portrait/#d84fc295915e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Strategie/Strategie.cpp Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,2389 @@
+#include "global.h"
+#include <string.h>
+#include <sstream>
+#include <math.h>
+#include <vector>
+//#include "StrategieManager.h"
+
+
+
+#define M_PI 3.14159265358979323846
+#define VERT 0xFF00FF00
+#define ROUGE 0xFFFF0000
+#define BLEU 0xFF0000FF
+#define JAUNE 0xFFFDD835//FEFE00
+#define BLANC 0xFF000000
+#define ORANGE 0xFFFFA500
+#define NOIR 0xFF000000
+#define DIY_GREY 0xFFDFDFDF
+#define VIOLET 0xFF4527A0
+
+char tableau_aff[10][50];
+char tableau_etat[22][50]= {
+ "Check_carte_screen",
+ "Check_carte_screen_wait_ack",
+ "Check_cartes",
+ "Check_cartes_wait_ack",
+ "Wait_force",
+ "Config",
+ "Game_init",
+ "Game_wait_for_jack",
+ "Game_start",
+ "Game_next_instruction",
+ "Game_instruction",
+ "Game_wait_ack",
+ "Game_jump_time",
+ "Game_jump_config",
+ "Game_jump_position",
+ "Game_wait_end_instruction",
+ "Warning_timeout",
+ "Waring_end_balise_wait",
+ "Warning_end_last_instruction",
+ "Warning_switch_strategie",
+ "End",
+ "End_loop",
+};
+
+int waitingAckID_FIN;
+int waitingAckFrom_FIN;
+
+Ticker ticker;
+TS_DISCO_F469NI ts;
+LCD_DISCO_F469NI lcd;
+
+TS_StateTypeDef TS_State;
+
+Ticker chrono;
+Timeout AffTime;
+Timer timer;
+Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
+Timer gameTimer;
+Timer debugetatTimer;
+Timer timeoutWarning;
+Timer timeoutWarningWaitEnd;
+Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin
+
+unsigned char screenChecktry = 0;
+unsigned char test[32] = {32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32};
+
+char counter = 0;
+char check;
+char Jack = 1;
+short SCORE_GLOBAL=0;
+short SCORE_GR=0;
+short SCORE_PR=0;
+
+int flag = 0, flag_strat = 0, flag_timer;
+int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0;
+char Ack_strat = 0;
+signed char Strat = 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
+signed short target_x_robot, target_y_robot, target_theta_robot;
+E_InstructionType actionPrecedente;
+signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise
+//unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+int flagSendCan=1;
+unsigned char Cote = 0; //0 -> JAUNE | 1 -> VIOLET
+unsigned short angleRecalage = 0;
+unsigned char checkCurrent = 0;
+unsigned char countAliveCard = 0;
+unsigned char ligne=0;
+
+
+int Fevitement=0;
+int EvitEtat= 0;
+int stop_evitement=0;
+
+
+
+
+
+signed char Strategie = 0; //N° de la strategie (1-10)
+
+unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo
+
+unsigned char countRobotNear = 0;//Le nombre de robot à proximité
+
+unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise
+unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
+short direction;
+
+unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
+
+struct S_Instruction instruction;
+struct S_Dodge_queue dodgeq;
+
+char couleur1, couleur2, couleur3;
+float cptf;
+int cpt,cpt1;
+
+typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_CAPTEURS, TEST_SERVO, TEST_TIR, DEMO_IMMEUBLE,DEMO_TRIEUR, 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;
+E_stratGameEtat lastEtat = ETAT_CHECK_CARTES;
+E_Stratposdebut etat_pos=RECALAGE_1;
+
+/////////////////DEFINITION DES BOUTONS////////////////////
+Button COTE_VERT(0, 25, 400, 300, "JAUNE");
+Button COTE_ORANGE(0, 350, 400, 300, "VIOLET");
+Button COTE_JAUNE(0, 25, 400, 300, "JAUNE");
+Button COTE_VIOLET(0, 350, 400, 300, "VIOLET");
+Button RETOUR (0, 680, 400, 110, "--Precedent--");
+Button LANCER (0, 200, 400, 200, "--LANCER--");
+Button CHECK (0, 420, 400, 200, "Valider");
+Button MATCH (0, 50, 400, 320, "Match");
+Button DEMONSTRATION (0, 400, 400, 320, "Demo");
+Button TEST_HERKULEX(0, 25, 400, 100, "Test servos");
+Button TEST_LASER(0, 135, 400, 100, "Test telemetre");
+Button TEST_COULEURS(0,245,400,100,"Test capteurs");
+Button TEST_TIR_BALLE(0,355,400,100,"Test Lanceur");
+Button TEST_IMMEUBLE(0,465,400,100,"Test immeuble");
+Button TEST_TRIEUR(0,575,400,100,"Test aiguilleur");
+Button TIR_CHATEAU(0, 25, 400, 100, "Tir chateau");
+Button EPURATION(0, 150, 400, 100, "epuration");
+Button LANCEUR_ON(0,275,400,100,"allumer le lanceur");
+Button LANCEUR_OFF(0,400,400,100,"eteindre le lanceur");
+Button ABAISSE_BLOC(0, 25, 400, 100, "Ramasser blocs");
+Button RELEVE_BLOC(0, 135, 400, 100, "lacher blocs");
+Button BRAS_ABEILLE_ON(0,245,400,100,"bras abeille");
+Button BRAS_ABEILLE_OFF(0,355,400,100,"baisser bras abeille");
+Button INTERRUPTEUR_ON(0,465,400,100,"baisser bras interrupt");
+Button INTERRUPTEUR_OFF(0,575,400,100,"baisser bras interrupt");
+Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
+Button TRI(0, 25, 400, 100, "Test tri");
+Button AIGUILLEUR_D(0, 150, 400, 100, "aiguilleur droite");
+Button AIGUILLEUR_G(0,275,400,100,"aiguilleur gauche");
+Button AIGUILLEUR_CTRE(0,400,400,100,"aiguilleur centre");
+Button SUIVANT(0,380,200,100,"Suivant");
+Button COLOR_ORANGE (0, 230, 190, 110,"");
+Button COLOR_JAUNE (210, 230, 190, 110,"");
+Button COLOR_VERT (0, 350, 190, 110,"");
+Button COLOR_BLEU (210, 350, 190, 110,"");
+Button COLOR_NOIR (105, 470, 190, 110,"");
+////////////////////////////////////////////////////////////
+
+void SendRawId (unsigned short id);
+void SelectionStrat (unsigned char numeroStrat);
+void Setflag(void);
+void can2Rx_ISR(void);
+signed char Bouton_Strat (void);
+signed char blocage_balise;
+void print_segment(int nombre, int decalage);
+void affichage_compteur (int nombre);
+void effacer_segment(long couleur);
+
+unsigned short telemetreDistance=0;
+unsigned short telemetreDistance_avant_gauche=0;
+unsigned short telemetreDistance_avant_droite=0;
+unsigned short telemetreDistance_arriere_gauche=0;
+unsigned short telemetreDistance_arriere_droite=0;
+
+unsigned char DT_AVD_interrupt=0;
+unsigned char DT_AVG_interrupt=0;
+unsigned char DT_ARD_interrupt=0;
+unsigned char DT_ARG_interrupt=0;
+
+
+#ifdef ROBOT_BIG
+
+
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
+
+InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
+#else
+
+
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
+InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
+
+
+#endif
+
+
+
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: chronometre_ISR */
+/* DESCRIPTION : Interruption à la fin des 90s du match */
+/****************************************************************************************/
+void chronometre_ISR (void)
+{
+ SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
+ SendRawId(GLOBAL_GAME_END);//Indication fin de match
+ etat=FIN;
+ gameTimer.stop();//Arret du timer
+
+ while(1);//On bloque la programme dans l'interruption
+}
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: jack_ISR */
+/* DESCRIPTION : Interruption en changement d'état sur le Jack */
+/****************************************************************************************/
+void jack_ISR (void)
+{
+ if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
+ gameEtat = ETAT_GAME_START;//On débute le match
+ //etat=COMPTEUR;
+ blocage_balise=1;
+ }
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: SelectionStrat */
+/* DESCRIPTION : Affiche la Stratégie sélectionnée sur l'ihm */
+/****************************************************************************************/
+
+
+void SelectionStrat (unsigned char Strategie)
+{
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+
+ switch (Strategie+1) {
+ case 0x1 :
+ //description de Strategie n°1
+ lcd.DisplayStringAt(150, 0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0x2 :
+ //description de Strategie n°2
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0x3 :
+ //description de Strategie n°3
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0x4 :
+ //description de Strategie n°4
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0x5 :
+ //description de Strategie n°5
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0x6 :
+ //description de Strategie n°5
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0x7 :
+ //description de Strategie n°5
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0x8 :
+ //description de Strategie n°5
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0x9 :
+ //description de Strategie n°5
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+
+ case 0xA :
+ //description de Strategie n°5
+ lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
+ break;
+ }
+}
+
+void Setflag(void)
+{
+ flagSendCan = 1;
+}
+
+
+//Affiche une variable sur l'écran tactile//
+void affichage_var(double Var)
+{
+ if(ligne==7)
+ ligne=0;
+ char aff[10]="toto";
+ sprintf(aff,"%lf ",Var);
+ lcd.DisplayStringAt(120, LINE(20+(ligne)), (uint8_t *)aff, LEFT_MODE);
+ //ligne++;
+
+}
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: affichage_debug */
+/* DESCRIPTION : Affiche l'état de gameEtat sur l'écran lcd */
+/****************************************************************************************/
+void affichage_debug(int Var)
+{
+ int i;
+ int conv=(int)Var;
+ SUIVANT.Draw(ROUGE, 0);
+ for(i=0; i<9; i++) {
+ strcpy(tableau_aff[i],"");
+ strcpy(tableau_aff[i],tableau_aff[i+1]);
+ }
+ strcpy(tableau_aff[9],tableau_etat[conv]);
+
+ for(i=0; i<10; i++) {
+ lcd.SetBackColor(VERT);
+ lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
+ }
+ /*while(!ack_bluetooth){ // mode pas à pas en bluetooth ou via écran
+ //liaison_bluetooth();
+ }
+ ack_bluetooth=0;*/
+ /*while(SUIVANT.Touched()==0);
+ while(SUIVANT.Touched());*/
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: automate_etat_ihm */
+/* DESCRIPTION : Automate de gestion de l'affichage */
+/****************************************************************************************/
+void automate_etat_ihm(void)
+{
+ int j;
+ if (j==0) {
+ ts.Init(lcd.GetXSize(), lcd.GetYSize());
+ j++;
+ }
+ ts.GetState(&TS_State);
+ switch (etat) {
+ case INIT : //intialise l'écran et passe à l'attente d'initialisation des cartes
+ ts.GetState(&TS_State);
+ canProcessRx();
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.Clear (LCD_COLOR_WHITE);
+ wait(0.15);
+ lcd.DisplayStringAt(0, 10, (uint8_t *)"Verification des cartes", LEFT_MODE);
+ //cartes non verifiées////////////////
+ lcd.SetTextColor(DIY_GREY);
+ lcd.FillRect(0,400,400,150); //carte moteur
+ lcd.FillRect(0,600,400,150); //Balise
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.SetBackColor(DIY_GREY);
+ lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
+ lcd.DisplayStringAt(110,650, (uint8_t *)"Balise", LEFT_MODE);
+ ////////////////////////////////////////
+ FORCE_LAUNCH.Draw(0xFFFF0000, 0);
+
+ etat=ATT;
+ break;
+
+ case ATT : //Si les cartes sont présentes passe directement à choix sinon attente de force Launch (cette partie est encore buggée mais les cartes affichent bien leur présence donc faut juste force launch tout le temps...)
+ if (flag==1) {
+ etat = CHOIX;
+ gameEtat = ETAT_CONFIG;
+ } else if (FORCE_LAUNCH.Touched()) {
+ etat = CHOIX;
+ gameEtat = ETAT_CONFIG;
+ while(FORCE_LAUNCH.Touched());
+ }
+
+ break;
+
+
+ case CHOIX : //Match ou DEMO
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.Clear (LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", LEFT_MODE);
+ DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
+ MATCH.Draw(0xFFF01010, 0);
+ while(etat == CHOIX) {
+ canProcessRx();
+ if(DEMONSTRATION.Touched()) {
+ etat = DEMO;
+ while(DEMONSTRATION.Touched());
+ }
+
+ if(MATCH.Touched()) {
+ etat = SELECT_SIDE;
+ while(MATCH.Touched());
+ }
+
+ }
+ break;
+
+ case DEMO :
+ lcd.Clear(LCD_COLOR_WHITE);
+ RETOUR.Draw(0xFFFF0000, 0);
+ TEST_HERKULEX.Draw(VERT, 0);
+ TEST_LASER.Draw(VERT, 0);
+ TEST_COULEURS.Draw(VERT, 0);
+ TEST_TIR_BALLE.Draw(VERT, 0);
+ TEST_IMMEUBLE.Draw(VERT,0);
+ TEST_TRIEUR.Draw(VERT,0);
+ if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config//
+ InversStrat = 0;//Pas d'inversion de la couleur
+ }
+ while (etat == DEMO) { ////////////////////////////LISTE DES DIFFERENTES DEMOS POSSIBLES///////////////////////////////////////////
+ canProcessRx();
+ if(TEST_HERKULEX.Touched()) {
+ //Strat = 0x10;
+ while(TEST_HERKULEX.Touched());
+ CANMessage trame_Tx = CANMessage();
+ trame_Tx.len = 1;
+ trame_Tx.format = CANStandard;
+ trame_Tx.type = CANData;
+ trame_Tx.id=CHOICE_COLOR;
+ trame_Tx.data[0]=0x2;
+ can2.write(trame_Tx);
+ TEST_HERKULEX.Draw(0xFFF0F0F0, 0);
+ etat = TEST_SERVO;
+ lcd.Clear(LCD_COLOR_WHITE);
+ ModeDemo=1;
+ } else if(TEST_LASER.Touched()) {
+ //Strat = 0x11;
+ while(TEST_LASER.Touched());
+ TEST_LASER.Draw(0xFFF0F0F0, 0);
+ etat = TEST_TELEMETRE;
+ } else if (TEST_COULEURS.Touched()) {
+ while(TEST_COULEURS.Touched());
+ TEST_LASER.Draw(0xFFF0F0F0, 0);
+ etat =TEST_CAPTEURS ;
+ }
+
+ else if (TEST_TIR_BALLE.Touched()) {
+ while(TEST_TIR_BALLE.Touched());
+ while(1) {
+ TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);
+ // rn42_Tx.printf("A");//experience
+#ifdef ROBOT_SMALL
+ liaison_Tx.envoyer_short(0x30,666);
+ pc.printf("data\r");
+#else
+
+ // lire();
+ /* if (bluetooth.readable())
+ pc.putc(bluetooth.getc());
+
+ if (pc.readable()) {
+ char c = pc.getc();
+ if (c == 'A') {
+ liaison.envoyer_short(PAQUET_IDENTIFIANT_RAFRAICHIRSCORE, 20);
+ pc.printf("rafraichir\n");
+ }
+ }*/
+#endif
+ }
+ //ModeDemo=1;
+ } else if(TEST_IMMEUBLE.Touched()) {
+ while(TEST_IMMEUBLE.Touched());
+ TEST_IMMEUBLE.Draw(0xFFF0F0F0, 0);
+ etat =DEMO_IMMEUBLE;
+ lcd.Clear(LCD_COLOR_WHITE);
+ } else if(TEST_TRIEUR.Touched()) {
+ while(TEST_TRIEUR.Touched());
+ etat=DEMO_TRIEUR;
+ lcd.Clear(LCD_COLOR_WHITE);
+ }
+ if(RETOUR.Touched()) {
+ etat = CHOIX;
+ while(RETOUR.Touched());
+
+ }
+ if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
+ Ack_strat = 1;
+ wait_ms(10);
+ }
+ }
+ break;
+
+
+ case TEST_SERVO: //TEST DU RESTE DES SERVOS DISPO HORS TIR
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
+ ABAISSE_BLOC.Draw(VERT, 0);
+ RELEVE_BLOC.Draw(VERT, 0);
+ BRAS_ABEILLE_ON.Draw(VERT, 0);
+ BRAS_ABEILLE_OFF.Draw(VERT, 0);
+ INTERRUPTEUR_ON.Draw(VERT, 0);
+ INTERRUPTEUR_OFF.Draw(VERT, 0);
+ RETOUR.Draw(0xFFFF0000,0);
+ while(etat==TEST_SERVO) {
+ if(RETOUR.Touched()) {
+ while (RETOUR.Touched());
+ etat=DEMO;
+ } else if(ABAISSE_BLOC.Touched()) {
+ while (ABAISSE_BLOC.Touched());
+ SendRawId(GABARIT_PETIT_ROBOT);
+ break;
+ } else if(RELEVE_BLOC.Touched()) {
+ while (RELEVE_BLOC.Touched());
+ SendRawId(PRESENTOIR_AVANT);
+ break;
+ } else if(BRAS_ABEILLE_ON.Touched()) {
+ while (BRAS_ABEILLE_ON.Touched());
+ //SendRawId(BRAS_ABEILLE_UP);
+ break;
+
+ } else if(BRAS_ABEILLE_OFF.Touched()) {
+ while (BRAS_ABEILLE_OFF.Touched());
+ //SendRawId(BRAS_ABEILLE_DOWN);
+ break;
+ } else if(INTERRUPTEUR_ON.Touched()) {
+ while (INTERRUPTEUR_ON.Touched());
+ //SendRawId(ALLUMER_PANNEAU_UP);
+ break;
+ } else if(INTERRUPTEUR_OFF.Touched()) {
+ while (INTERRUPTEUR_OFF.Touched());
+ //SendRawId(ALLUMER_PANNEAU_DOWN);
+ break;
+ }
+ }
+ break;
+
+ case TEST_TIR: // TEST DES FONCTIONS LIEES AUX TIRS
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
+ TIR_CHATEAU.Draw(VERT, 0);
+ EPURATION.Draw(VERT, 0);
+ LANCEUR_ON.Draw(VERT, 0);
+ LANCEUR_OFF.Draw(VERT, 0);
+ RETOUR.Draw(ROUGE, 0);
+
+ etat=DEMO;
+
+ break;
+
+
+
+ case TEST_TELEMETRE: //AFFICHAGE DE LA VALEUR LUE PAR LES 4 TELEMETRES
+ ModeDemo=1;
+ lcd.Clear(LCD_COLOR_WHITE);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
+ RETOUR.Draw(0xFFFF0000, 0);
+ while(etat==TEST_TELEMETRE) {
+ SendRawId(DATA_RECALAGE);
+ SendRawId(DATA_TELEMETRE_LOGIQUE);
+ wait_ms(100);
+ canProcessRx();
+ if(RETOUR.Touched()) {
+ while( RETOUR.Touched());
+ etat=DEMO;
+ lcd.Clear(LCD_COLOR_WHITE);
+ }
+ }
+ break; ///////////////////////////////////////////FIN DES DEMOS/////////////////////////////////////////////////
+
+
+ case SELECT_SIDE : // CHOIX DU COTE DU TERRAIN + INVERSION DE LA STRAT SI COTE ORANGE+ ENVOI DU COTE A LA CARTE CAPTEUR/ACTIONNEURS
+ lcd.Clear(LCD_COLOR_WHITE);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+
+ lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE);
+ COTE_JAUNE.Draw(JAUNE, 0);
+ COTE_VIOLET.Draw(VIOLET, 0);
+ RETOUR.Draw(LCD_COLOR_RED, 0);
+
+
+ while (etat == SELECT_SIDE) {
+ canProcessRx();
+ if(COTE_JAUNE.Touched()) {
+ Cote = 0x0;
+ InversStrat = Cote;
+ etat = TACTIQUE;
+ CANMessage trame_Tx = CANMessage();
+ trame_Tx.len = 1;
+ trame_Tx.format = CANStandard;
+ trame_Tx.type = CANData;
+ trame_Tx.id=CHOICE_COLOR;
+ trame_Tx.data[0]=Cote;
+ can2.write(trame_Tx);
+ while(COTE_JAUNE.Touched());
+
+ }
+
+ if(COTE_VIOLET.Touched()) {
+ Cote = 0x1;
+ InversStrat= Cote;
+ etat = TACTIQUE;
+ CANMessage trame_Tx = CANMessage();
+ trame_Tx.len = 1;
+ trame_Tx.format = CANStandard;
+ trame_Tx.type = CANData;
+ trame_Tx.id=CHOICE_COLOR;
+ trame_Tx.data[0]=Cote;
+ can2.write(trame_Tx);
+ while(COTE_VIOLET.Touched());
+ }
+
+ if(RETOUR.Touched()) {
+ etat = CHOIX;
+ while(RETOUR.Touched());
+ }
+ }
+
+ break;
+
+ case TACTIQUE : //AFFICHE LA LISTE DES STRATS AFIN DE SELECTIONNER CELLE VOULUE
+ if (Cote == 0) {
+ lcd.Clear(JAUNE);
+ lcd.SetBackColor(JAUNE);
+ } else if (Cote == 1) {
+ lcd.Clear(VIOLET);
+ lcd.SetBackColor(VIOLET);
+ } else {
+ lcd.Clear(BLEU);
+ lcd.SetBackColor(BLEU);
+ }
+
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+
+ lcd.DisplayStringAt(20, LINE(0), (uint8_t *)"Choisir une strategie", LEFT_MODE);
+
+ Strategie = Bouton_Strat(); // retourne valeur de Strategie si bouton strat renvoi -1 on reviens en arriere
+ if (Strategie == -1) {
+ etat = SELECT_SIDE;
+ } else {
+ etat = DETAILS;
+ }
+ wait(0.1);
+ break;
+
+ case DETAILS : //SECONDE VALIDATION DE LA STRAT
+ lcd.Clear(LCD_COLOR_WHITE);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ CHECK.Draw(VERT);
+ RETOUR.Draw(LCD_COLOR_RED);
+
+ SelectionStrat(Strategie); //affiche la stratégie selectionnée
+
+ while (etat == DETAILS) {
+ canProcessRx();
+ if (CHECK.Touched()) {
+ if(gameEtat == ETAT_CONFIG) {
+ gameEtat = ETAT_GAME_INIT;
+ etat=LECTURE;
+
+ }
+ while(CHECK.Touched());
+ }
+
+ if(RETOUR.Touched()) {
+ etat = TACTIQUE;
+ while(RETOUR.Touched());
+ }
+ }
+ break;
+
+
+ case LECTURE :
+ break;
+ case AFF_WAIT_JACK : //FONCTIONS D'AFFICHAGE DE L'ATTENTE DU JACK
+ lcd.Clear(BLANC);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+
+ if (Cote == 0) {
+ lcd.Clear(VERT);
+ lcd.SetBackColor(VERT);
+ } else if (Cote == 1) {
+ lcd.Clear(ORANGE);
+ lcd.SetBackColor(ORANGE);
+ } else {
+ lcd.Clear(VERT);
+ lcd.SetBackColor(VERT);
+ }
+ canProcessRx();
+ lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
+ etat=WAIT_JACK;
+ break;
+
+ case WAIT_JACK: //VERITABLE ATTENTE DU JACK
+ break;
+
+ case COMPTEUR: //PEUT AFFICHER UN COMPTEUR DU TEMPS RESTANT AVANT LA FIN DE LA PARTIE OU BIEN TRES UTILE POUR PRINT DES VARIABLES CHAQUE SEC EX: gameEtat
+ cptf=gameTimer.read();
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ cpt=(int)cptf;
+ if(cpt != cpt1) {
+ lcd.Clear(VERT);
+ // affichage_compteur(100-cpt);
+ //affichage_compteur(SCORE_PR);
+#ifdef ROBOT_BIG
+ affichage_var(SCORE_GR);
+#else
+ affichage_var(SCORE_PR);
+#endif
+ if(liaison_pr.paquet_en_attente()) {
+ PaquetDomotique *paquet=liaison_pr.lire();
+ if(paquet->identifiant==PAQUET_IDENTIFIANT_AJOUTERSCORE) {
+ SCORE_PR+=convertir_score(paquet);
+ }
+ delete paquet;
+ }
+ }
+ cpt1=cpt;
+ flag_timer=0;
+
+ //affichage_debug(gameEtat);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+
+ break;
+
+ case FIN : //AFFICHAGE DE FIN AVEC LE SCORE FINAL
+ lcd.Clear (LCD_COLOR_WHITE);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+#ifdef ROBOT_BIG
+ // affichage_compteur(SCORE_GR);
+ affichage_var(SCORE_GR);
+ //liaison_Tx.envoyer_short(PAQUET_IDENTIFIANT_FINMATCH,SCORE_GLOBAL);
+#else
+ //affichage_compteur(SCORE_PR);
+ affichage_var(SCORE_PR);
+#endif
+ while(1); // force le redemarage du robot
+ //break;
+
+ }
+}
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: automate_process */
+/* DESCRIPTION : Automate de gestion de la stratégie du robot */
+/****************************************************************************************/
+void automate_process(void)
+{
+ static unsigned char AX12_enchainement = 0;
+ static unsigned char MV_enchainement = 0;
+ signed char localData1 = 0;
+ signed short localData2 = 0;
+ unsigned short localData3 = 0;
+ //signed short localData4 = 0;
+ unsigned char localData5 = 0;
+
+
+ if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments
+ gameTimer.stop();
+ gameTimer.reset();
+ gameEtat = ETAT_END;//Fin du temps
+ etat=FIN;
+ }
+
+ if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
+ lastEtat = gameEtat;
+ debugetatTimer.reset();
+ sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
+ }
+
+
+
+ switch(gameEtat) {
+
+ case ETAT_CHECK_CARTES:
+ /*
+ Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
+ */
+ waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
+ SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence
+
+ screenChecktry++;//On incrèment le conteur de tentative de 1
+ cartesCheker.reset();//On reset le timeOut
+ cartesCheker.start();//On lance le timer pour le timeout
+ gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
+ break;
+
+ case ETAT_CHECK_CARTES_WAIT_ACK:
+ /*
+ On attend l'ack de la carte en cours de vérification
+ */
+ //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
+ if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
+ cartesCheker.stop();
+ screenChecktry = 0;
+ countAliveCard++;
+ checkCurrent++;
+ if(checkCurrent >= NOMBRE_CARTES) {
+ printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
+ if(countAliveCard >= NOMBRE_CARTES) {
+ gameEtat = ETAT_CONFIG;
+ SendRawId(ECRAN_ALL_CHECK);
+ flag=1;
+
+ //tactile_printf("Selection couleur et strategie");
+ } else {
+ gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
+ waitingAckFrom = ECRAN_ALL_CHECK;
+ }
+ } else
+ gameEtat = ETAT_CHECK_CARTES;
+ } else if(cartesCheker.read_ms () > 100) {
+ cartesCheker.stop();
+ if(screenChecktry >=3) {
+ //printf("missing card %d\n",id_check[checkCurrent]);
+ screenChecktry = 0;
+ checkCurrent++;
+
+ if(checkCurrent >= NOMBRE_CARTES) {
+ if(countAliveCard == NOMBRE_CARTES) {
+ gameEtat = ETAT_CONFIG;
+ flag=1;
+ } else {
+ gameEtat = ETAT_WAIT_FORCE;
+ waitingAckFrom = ECRAN_ALL_CHECK;
+ }
+ } else
+ gameEtat = ETAT_CHECK_CARTES;
+
+ } else
+ gameEtat = ETAT_CHECK_CARTES;
+
+ }
+ break;
+ case ETAT_WAIT_FORCE:
+ /*
+ Attente du forçage de la part de la carte IHM
+ */
+ if(waitingAckFrom == 0) {
+ gameEtat = ETAT_CONFIG;
+ }
+ break;
+ case ETAT_CONFIG:
+ /*
+ Attente de l'odre de choix de mode,
+ Il est possible de modifier la couleur et l'id de la stratégie
+ Il est aussi possible d'envoyer les ordres de debug
+ */
+ modeTelemetre = 0;
+ break;
+ case ETAT_GAME_INIT:
+ //On charge la liste des instructions
+
+ loadAllInstruction(Strategie);//Mise en cache de toute les instructions
+ led3=1;
+
+ SendRawId(GLOBAL_START);
+
+ gameEtat = ETAT_GAME_WAIT_FOR_JACK;
+ if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE) {
+ SendRawId(DEBUG_FAKE_JAKE);
+ } else {
+ etat = AFF_WAIT_JACK;
+ }
+ //tactile_printf("Attente du JACK.");
+ setAsservissementEtat(1);//On réactive l'asservissement
+ jack.mode(PullDown); // désactivation de la résistance interne du jack
+ jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
+
+ localData2 = POSITION_DEBUT_T;
+ localData3 = POSITION_DEBUT_Y;
+ if(InversStrat == 1) {
+ localData2 = -localData2;//Inversion theta
+ localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
+ }
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,1800,localData2);
+
+ instruction = strat_instructions[actual_instruction];
+ //On effectue le traitement de l'instruction
+
+ break;
+ case ETAT_GAME_WAIT_FOR_JACK:
+ if(instruction.order==POSITION_DEBUT) {
+ switch(etat_pos) { // AUTOMATE PERMETTANT AU ROBOT DE SE POSITIONNER TOUT SEUL AU DEBUT DE LA PARTIE (Ne PAS RETIRER LE JACK PENDANT CE TEMPS !!!)
+ case RECALAGE_1:
+ SendRawId(RECALAGE_START);
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+#ifdef ROBOT_SMALL
+ GoStraight(3000, 1,MOITIEE_ROBOT, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot (les -5 qui trainent sont dus au tables pourraves sur place)
+#else
+ GoStraight(-3000, 1,MOITIEE_ROBOT, 0);
+#endif
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+ waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN= INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+ etat_pos=RECULER_1;
+ break;
+
+ case RECULER_1:
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+#ifdef ROBOT_SMALL
+ GoStraight(-100, 0, 0, 0);//-450
+#else
+ GoStraight(250, 0, 0, 0);
+#endif
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+ waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+ etat_pos=TOURNER;
+ break;
+
+ case TOURNER:
+ waitingAckID = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ if(Cote==0) {
+ localData2 = 900;
+ } else {
+ localData2=-900;
+ }
+ Rotate(localData2);
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+ waitingAckID_FIN=ASSERVISSEMENT_ROTATION;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+ etat_pos=RECALAGE_2;
+ break;
+
+ case RECALAGE_2:
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ if(Cote==1) {
+ localData3=3000-(MOITIEE_ROBOT);
+ } else {
+ localData3=MOITIEE_ROBOT;
+ }
+#ifdef ROBOT_SMALL
+ GoStraight(3000, 2,localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot
+#else
+ GoStraight(-3000, 2,localData3, 0);
+#endif
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+ waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+ etat_pos=RECULER_2;
+ break;
+
+ case RECULER_2:
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+#ifdef ROBOT_SMALL
+ GoStraight(-100, 0, 0, 0);
+#else
+ GoStraight(250, 0, 0, 0);
+#endif
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+ waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+ etat_pos=GOTOPOS;
+ break;
+
+ case GOTOPOS:
+ localData1 = -1;
+
+ if(InversStrat == 1 && ingnorInversionOnce == 0) {
+ localData2 = -instruction.arg3;
+ localData3 = 3000 - instruction.arg2;//Inversion du Y
+ } else {
+ localData3 = instruction.arg2;
+ localData2 = instruction.arg3;
+ }
+
+ GoToPosition(instruction.arg1,localData3,localData2,localData1);
+ waitingAckID = ASSERVISSEMENT_XYT;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+
+ while(waitingAckID !=0 && waitingAckFrom !=0)
+ canProcessRx();
+ waitingAckID_FIN=ASSERVISSEMENT_XYT;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
+ canProcessRx();
+ etat_pos=FIN_POS;
+ break;
+ case FIN_POS:
+ actual_instruction = instruction.nextLineOK;
+ break;
+ }
+ }
+
+
+ break;
+ case ETAT_GAME_START:
+
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+
+ if (ModeDemo == 0) {
+ chronoEnd.attach(&chronometre_ISR,100);//On lance le chrono de 90s
+ gameTimer.start();
+ }
+ gameTimer.reset();
+ jack.fall(NULL);//On désactive l'interruption du jack
+ //SendRawId(GLOBAL_START);
+ Jack=0; //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV
+ //tactile_printf("Start");//Pas vraiment utile mais bon
+ break;
+ case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
+ flagNonRepriseErrorMot = 0;
+ /*
+ Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
+ */
+ //printf("load next instruction\n");
+ if(dodgeq.nb > 0){//dodge q
+ instruction.order=dodgeq.inst[dodgeq.nb-1].order;
+ instruction.arg1=dodgeq.inst[dodgeq.nb-1].arg1;
+ instruction.arg2=dodgeq.inst[dodgeq.nb-1].arg2;
+ instruction.arg3=dodgeq.inst[dodgeq.nb-1].arg3;
+ gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ dodgeq.nb--;
+ }//end dodge q
+ else{// no dodge q
+ if(actual_instruction >= nb_instructions || actual_instruction == 255) {
+ gameEtat = ETAT_END;
+ //Il n'y a plus d'instruction, fin du jeu
+ } else {
+ instruction = strat_instructions[actual_instruction];
+ //On effectue le traitement de l'instruction
+ gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
+ }
+ }//end no dodge q
+ screenChecktry = 0;
+ ingnorInversionOnce = 0;
+ break;
+ case ETAT_GAME_PROCESS_INSTRUCTION:
+ /*
+ Traitement de l'instruction, envoie de la trame CAN
+ */
+ //debug_Instruction(instruction);
+ //affichage_debug(gameEtat);
+ //rn42_Tx.printf("A");//lance l'electron
+ actionPrecedente = instruction.order;
+ switch(instruction.order) {
+ case MV_BEZIER: {
+ static vector< vector<short> >P1;
+ static vector< vector<short> >C1;
+ static vector< vector<short> >C2;
+ static int i = 0;
+
+ //Ajoute une ligne aux tableaux pour chaques courbes de la trajectoire
+ P1.push_back(vector<short>()); //Nouvelle ligne
+ C1.push_back(vector<short>()); //Nouvelle ligne
+ C2.push_back(vector<short>()); //Nouvelle ligne
+
+ P1[i].push_back(instruction.arg1); //Nouvelle colonne X
+ C1[i].push_back(instruction.arg3); //Nouvelle colonne X
+ C2[i].push_back(instruction.arg5); //Nouvelle colonne X
+
+ if(InversStrat == 1 && ingnorInversionOnce == 0) {
+ P1[i].push_back(3000-instruction.arg2); //Nouvelle colonne Y
+ C1[i].push_back(3000-instruction.arg4); //Nouvelle colonne Y
+ C2[i].push_back(3000-instruction.arg6); //Nouvelle colonne Y
+ } else {
+ P1[i].push_back(instruction.arg2); //Nouvelle colonne Y
+ C1[i].push_back(instruction.arg4); //Nouvelle colonne Y
+ C2[i].push_back(instruction.arg6); //Nouvelle colonne Y
+ }
+
+ i++;
+
+ if(instruction.nextActionType == WAIT) { //Si il n'y a qu'une seule courbe ou que c'est la dernière courbe de la trajectoire
+ //Passage des points dans des variables temporaires pour pouvoir clear les vector avant d'être bloqué dans l'attente de l'ack
+ //Empeche les vector de ne pas être reset si l'ack n'est pas reçu avant la fin du match
+ int nbCourbes = P1.size();
+ short P1_temp[nbCourbes][2];
+ short C1_temp[nbCourbes][2];
+ short C2_temp[nbCourbes][2];
+
+ for(int j=0; j<nbCourbes; j++) {
+ for(int i=0; i<2; i++) {
+ P1_temp[j][i] = P1[j][i];
+ C1_temp[j][i] = C1[j][i];
+ C2_temp[j][i] = C2[j][i];
+ }
+ }
+
+ //Clear des tableaux de points pour la prochaine trajectoire
+ P1.clear();
+ C1.clear();
+ C2.clear();
+ i = 0;
+
+ //Calcul de la courbe et envoi des valeurs
+ if(instruction.direction == FORWARD) {
+ courbeBezier(nbCourbes, P1_temp, C1_temp, C2_temp, 0);
+ } else if(instruction.direction == BACKWARD) {
+ courbeBezier(nbCourbes, P1_temp, C1_temp, C2_temp, 1);
+ }
+
+ waitingAckID = ASSERVISSEMENT_BEZIER;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ }
+ break;
+ }
+ case MV_COURBURE://C'est un rayon de courbure
+ float alpha=0, theta=0;
+ unsigned short alph=0;
+ actionPrecedente = MV_COURBURE;
+ waitingAckID = ASSERVISSEMENT_COURBURE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ if(instruction.nextActionType == ENCHAINEMENT) {
+ MV_enchainement++;
+ localData5 = 1;
+ } else {
+ if(MV_enchainement > 0) {
+ localData5 = 2;
+ MV_enchainement = 0;
+ } else {
+ localData5 = 0;
+ }
+ }
+ if(InversStrat == 1 && ingnorInversionOnce == 0) {
+ if(instruction.direction == LEFT) instruction.direction = RIGHT;
+ else instruction.direction = LEFT;
+ }
+
+ localData1 = ((instruction.direction == LEFT)?1:-1);
+ localData2 = instruction.arg3;
+ /*if(InversStrat == 1 && ingnorInversionOnce == 0) {
+ localData1 = -localData1;//Inversion de la direction
+ }*/
+
+ BendRadius(instruction.arg1, localData2, localData1, localData5);
+#ifdef ROBOT_SMALL
+ if(localData2>0) {
+ direction=1;
+ } else {
+ direction=-1;
+ }
+#else
+ if(localData2>0) {
+ direction=-1;
+ } else {
+ direction=1;
+ }
+#endif
+ if(localData2>0)alph=localData2;
+ else alph=-localData2;
+ alpha = localData2*M_PI/1800.0f;
+ theta = theta_robot*M_PI/1800.0f;
+
+ if(instruction.direction == LEFT) { //-------------LEFT
+ if(alph<450){ // 1 XYT
+ dodgeq.inst[0].order = MV_XYT;
+ dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
+ dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
+ dodgeq.inst[0].arg3 = theta_robot + alph;// T
+ }
+ else if(alph<900){
+ for(int c=0;c<2;c++){ // 2 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot + alph;// T
+ alpha-=alpha/2.0f;
+ alph-=alph/2;
+ }
+ }
+ else if(alph<1350){
+ for(int c=0;c<3;c++){ // 3 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot + alph;// T
+ alpha-=alpha/3.0f;
+ alph-=alph/3;
+ }
+ }
+ else if(alph<1800){
+ for(int c=0;c<4;c++){ // 4 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot + alph;// T
+ alpha-=alpha/4.0f;
+ alph-=alph/4;
+ }
+ }
+ else if(alph<2250){
+ for(int c=0;c<5;c++){ // 5 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot + alph;// T
+ alpha-=alpha/5.0f;
+ alph-=alph/5;
+ }
+ }
+ else {
+ for(int c=0;c<6;c++){ // 6 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot + alph;// T
+ alpha-=alpha/6.0f;
+ alph-=alph/6;
+ }
+ }
+ } else { //-----------------------------------------RIGHT
+ if(alph<450){ // 1 XYT
+ dodgeq.inst[0].order = MV_XYT;
+ dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
+ dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
+ dodgeq.inst[0].arg3 = theta_robot - alph;// T
+ }
+ else if(alph<900){
+ for(int c=0;c<2;c++){ // 2 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot - alph;// T
+ alpha-=alpha/2.0f;
+ }
+ }
+ else if(alph<1350){
+ for(int c=0;c<3;c++){ // 3 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot - alph;// T
+ alpha-=alpha/3.0f;
+ }
+ }
+ else if(alph<1800){
+ for(int c=0;c<4;c++){ // 4 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot - alph;// T
+ alpha-=alpha/4.0f;
+ }
+ }
+ else if(alph<2250){
+ for(int c=0;c<5;c++){ // 5 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot - alph;// T
+ alpha-=alpha/5.0f;
+ }
+ }
+ else {
+ for(int c=0;c<6;c++){ // 6 points de passages
+ dodgeq.inst[c].order = MV_XYT;
+ dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X
+ dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y
+ dodgeq.inst[c].arg3 = theta_robot - alph;// T
+ alpha-=alpha/6.0f;
+ }
+ }
+ }
+ break;
+
+
+
+ case MV_LINE://Ligne droite
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ if(instruction.nextActionType == ENCHAINEMENT) {
+ MV_enchainement++;
+ localData5 = 1;
+ } else {
+ if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
+ localData5 = 2;
+ MV_enchainement = 0;
+ } else {
+ localData5 = 0;
+ }
+ }
+ localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
+ GoStraight(localData2, 0, 0, localData5);
+
+ target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
+ target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
+ target_theta_robot = theta_robot;
+
+ break;
+ case MV_TURN: //Rotation sur place
+ target_x_robot = x_robot;
+ target_y_robot = y_robot;
+ target_theta_robot = theta_robot + localData2;
+ localData2 = instruction.arg3;
+
+ if(InversStrat == 1 && ingnorInversionOnce == 0) {
+ localData2 = -localData2;
+ }
+
+
+ if(instruction.direction == ABSOLUTE) {
+ //C'est un rotation absolu, il faut la convertir en relative
+
+ localData2 = (localData2 - theta_robot)%3600;
+ if(localData2 > 1800) localData2 = localData2-3600;
+
+ else if(localData2 <-1800) localData2 = localData2+3600;
+ }
+
+
+ waitingAckID = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+ Rotate(localData2);
+
+ break;
+ case MV_XYT:
+ if(instruction.direction == BACKWARD) {
+ localData1 = -1;
+ } else {
+ localData1 = 1;
+ }
+
+ if(InversStrat == 1 && ingnorInversionOnce == 0) {
+ localData2 = -instruction.arg3;
+ localData3 = 3000 - instruction.arg2;//Inversion du Y
+ } else {
+ localData3 = instruction.arg2;
+ localData2 = instruction.arg3;
+ }
+
+ GoToPosition(instruction.arg1,localData3,localData2,localData1);
+ waitingAckID = ASSERVISSEMENT_XYT;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+
+ target_x_robot = instruction.arg1;
+ target_y_robot = localData3;
+ target_theta_robot = localData2;
+
+ break;
+ case MV_RECALAGE:
+ if(instruction.nextActionType == MECANIQUE) {
+ instruction.nextActionType = WAIT;
+
+ waitingAckID = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+
+ localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
+
+ if(instruction.precision == RECALAGE_Y) {
+ localData5 = 2;
+ if(InversStrat == 1 && ingnorInversionOnce == 0) {
+ localData3 = 3000 - instruction.arg1;//Inversion du Y
+ } else {
+ localData3 = instruction.arg1;
+ }
+ } else {
+ localData5 = 1;
+ localData3 = instruction.arg1;
+ }
+ GoStraight(localData2, localData5, localData3, 0);
+ } else { //CAPTEUR
+ SendRawId(DATA_RECALAGE);
+ waitingAckID = RECEPTION_RECALAGE;
+ waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
+
+ // On attend que les variables soient actualisé
+ while(!(waitingAckID == 0 && waitingAckFrom == 0))
+ canProcessRx();
+ while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0))
+ canProcessRx();
+
+ if(instruction.precision == RECALAGE_Y) { // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900)
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot);
+ } else if(instruction.precision == RECALAGE_X) {
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
+ } else if(instruction.precision == RECALAGE_T) {
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
+ }
+ }
+ break;
+
+ case ACTION:
+
+ waitingAckID_FIN = 0;
+ waitingAckFrom_FIN = 0;
+
+ 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);
+ // unsigned char test=(unsigned char) tempo;
+ // SendMsgCan(0x5BD, &test,1);
+ if(tempo == 1) {
+ //L'action est spécifique
+ if((waitingAckFrom == 0 && waitingAckID == 0) && instruction.nextActionType == ENCHAINEMENT) {
+ 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;
+#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++;
+
+ }
+ break;
+ default:
+ //Instruction inconnue, on l'ignore
+ break;
+ }
+
+ if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
+ gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
+ screenChecktry++;//On incrèment le conteur de tentative de 1
+ cartesCheker.reset();//On reset le timeOut
+ cartesCheker.start();
+ if(AX12_enchainement > 0) {
+ //AX12_processChange();//Il faut lancer le déplacement des AX12
+ //AX12_enchainement = 0;
+ }
+ } else { //C'est un enchainement
+ if(instruction.order == MV_LINE) {
+ gameEtat = ETAT_GAME_WAIT_ACK;
+ } else {
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
+ }
+ }
+
+ break;
+ case ETAT_GAME_WAIT_ACK:
+ canProcessRx();
+ //SendSpeed(200);//--------------------------------------------------MODE RALENTI !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+ if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
+ //if(true) {
+ cartesCheker.stop();
+ if(instruction.nextActionType == JUMP) {
+ if(instruction.jumpAction == JUMP_POSITION) {
+ gameEtat = ETAT_GAME_JUMP_POSITION;
+ } else { //Pour eviter les erreurs, on dit que c'est par défaut un jump time
+ gameEtat = ETAT_GAME_JUMP_TIME;
+ cartesCheker.reset();//On reset le timeOut
+ cartesCheker.start();
+ }
+ } else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions
+ /*wait_ms(200);
+ #ifdef ROBOT_BIG
+ SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot);
+ #else
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, theta_robot);
+ #endif
+ wait_ms(200);*/
+
+ gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
+ switch(instruction.order) {
+ case MV_BEZIER:
+ waitingAckID_FIN = ASSERVISSEMENT_BEZIER;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ break;
+ case MV_COURBURE:
+ waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ break;
+ case MV_LINE:
+ waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ break;
+ case MV_TURN:
+ waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ break;
+ case MV_XYT:
+ waitingAckID_FIN = ASSERVISSEMENT_XYT;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ break;
+ case MV_RECALAGE:
+ waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ break;
+ case ACTION:
+
+ if (modeTelemetre == 0) {
+ if (telemetreDistance == 0) {
+ waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
+ waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
+ } else if(telemetreDistance == 5000) {
+ // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
+ //waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
+ //waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
+ telemetreDistance = 0;
+ }
+ } else { // si on attend la reponse du telemetre
+ //modeTelemetre = 1;
+ waitingAckID_FIN = OBJET_SUR_TABLE;
+ waitingAckFrom_FIN = 0;
+ }
+ break;
+ default:
+ break;
+ }
+ } else {
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ }
+ } else if(cartesCheker.read_ms () > 1000) {
+ cartesCheker.stop();
+ if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
+ actual_instruction = instruction.nextLineError;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ } else {
+ gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
+ }
+ }
+ break;
+
+ case ETAT_GAME_JUMP_TIME:
+ if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
+ cartesCheker.stop();//On arrete le timer
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
+ }
+ break;
+
+ case ETAT_GAME_JUMP_CONFIG:
+ signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées
+ // 1 si l'instruction est plus grande que la position du robot
+ // -1 si l'instruction est plus petite que la position du robot
+ // 0 si l'instruction et position du robot sont proche de moins de 1cm
+ if (abs(x_robot-instruction.JumpTimeOrX)<10) {
+ depasX = 0;
+ } else if(x_robot > instruction.JumpTimeOrX) {
+ depasX = -1;
+ }
+
+ if(abs(y_robot-instruction.JumpY)<10) {
+ depasY = 0;
+ } else if(y_robot > instruction.JumpY) {
+ depasY = -1;
+ }
+
+ gameEtat = ETAT_GAME_JUMP_POSITION;
+ break;
+ case ETAT_GAME_JUMP_POSITION:
+ bool Xok = false, Yok = false;
+
+ if (depasX == 0) {
+ Xok = true;
+ } else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5) {
+ Xok = true;
+ }
+
+ if (depasY == 0) {
+ Yok = true;
+ } else if ((instruction.JumpY - y_robot)*depasY < -5) {
+ Yok = true;
+ }
+
+ // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
+ if (Xok && Yok) {
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
+ }
+
+ break;
+ case ETAT_GAME_WAIT_END_INSTRUCTION:
+ canProcessRx();
+ if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
+ }
+
+ break;
+
+ case ETAT_END:
+ if (ModeDemo) {
+ gameEtat = ETAT_CHECK_CARTE_SCREEN;
+ ModeDemo = 1;
+ } else {
+ gameEtat = ETAT_END_LOOP;
+ }
+ break;
+ case ETAT_END_LOOP:
+ //Rien, on tourne en rond
+
+ break;
+ default:
+
+ break;
+ }
+}
+
+
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx */
+/* DESCRIPTION : Fait évoluer l'automate de l'IHM en fonction des receptions sur le CAN*/
+/****************************************************************************************/
+void canProcessRx(void)
+{
+ static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+ char message[10]="toto";
+ char message1[10]="toto";
+ char message2[10]="toto";
+ char message3[10]="toto";
+ char message4[10]="toto";
+ FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+
+ if(FIFO_occupation<0)
+ FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+
+ if(FIFO_max_occupation<FIFO_occupation) {
+ FIFO_max_occupation=FIFO_occupation;
+ //SendRawId(
+ }
+
+ if(FIFO_occupation!=0) {
+ int identifiant=msgRxBuffer[FIFO_lecture].id;
+
+ if (waitingId == identifiant) waitingId = 0;
+ switch(identifiant) {
+
+ case ALIVE_MOTEUR:
+ if (etat == ATT) {
+
+ lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
+ lcd.FillRect(0,400,400,150);
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
+ lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
+ }
+ break;
+
+ case ALIVE_BALISE:
+ if (etat == ATT) {
+
+ lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
+ lcd.FillRect(0,600,400,150); //carte AX12
+ lcd.SetTextColor(LCD_COLOR_BLACK);
+ lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
+ lcd.DisplayStringAt(110, 650, (uint8_t *)"Balise", LEFT_MODE);
+ }
+ break;
+
+ case RESET_IHM:
+ etat = CHOIX;
+ break;
+
+ case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
+ case GLOBAL_JACK:
+ if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
+ gameEtat = ETAT_GAME_START;
+ SendRawId(ACKNOWLEDGE_JACK);
+ }
+ break;
+
+ case ALIVE_ACTIONNEURS_AVANT: //pas de break donc passe directement dans ECRAN_ALL_CHECK mais conserve l'ident initial
+ case ALIVE_ACTIONNEURS_ARRIERE:
+ case ALIVE_HERKULEX:
+ case ECRAN_ALL_CHECK:
+ if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
+ waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
+ }
+ flag=1;
+ break;
+
+ case ASSERVISSEMENT_ERROR_MOTEUR://erreur asservissement
+ {
+
+ unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
+ memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
+ if(recieveAckID == waitingAckID_FIN && waitingAckFrom_FIN == INSTRUCTION_END_MOTEUR)
+ {
+ if(flagNonRepriseErrorMot) {
+ actual_instruction = instruction.nextLineError;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ flagNonRepriseErrorMot = 0;
+ } else {
+ flagNonRepriseErrorMot = 1;
+ timeoutWarningWaitEnd.reset();
+ timeoutWarningWaitEnd.start();
+ //gameEtat = ETAT_WARING_END_BALISE_WAIT;
+ }
+ }
+ /*
+ if(flagNonRepriseErrorMot) {
+ actual_instruction = instruction.nextLineError;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ flagNonRepriseErrorMot = 0;
+ } else {
+ flagNonRepriseErrorMot = 1;
+ gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
+ }*/
+ } break;
+
+ /////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
+ case ACKNOWLEDGE_HERKULEX:
+ case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans ACK_FIN_ACTION mais conserve l'ident initial
+
+ case ACKNOWLEDGE_TELEMETRE:
+ /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
+ case ACKNOWLEDGE_MOTEUR:
+ case INSTRUCTION_END_BALISE:
+ case ACK_FIN_ACTION:
+ case INSTRUCTION_END_MOTEUR:
+ unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
+ memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
+ /*
+//on desactive la balise dans les rotations XYT
+ if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_XYT==recieveAckID)ingnorBalise=1;
+ if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_XYT_ROTATE==recieveAckID)ingnorBalise=0;
+
+//on desactive la balise dans les rotations
+ if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=1;
+ if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=0;
+ */
+
+ // SendMsgCan(0x666,&ingnorBalise,1);
+
+ if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+ }
+ if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) {
+ waitingAckFrom_FIN = 0;
+ waitingAckID_FIN = 0;
+ }
+
+ /*
+ 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;
+ }
+ if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]
+ |(((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN))
+ {
+ waitingAckFrom_FIN = 0;
+ waitingAckID_FIN = 0;
+ }
+ */
+ break;
+
+
+#ifdef ROBOT_BIG
+#define point_balance 12
+#define point_accelerateur 10
+ case NB_PALETS_BLEU: //nb de palets bleu mis dans l'accelerateur
+ unsigned short palets_bleu=(unsigned short)msgRxBuffer[FIFO_lecture].data[0];
+ SCORE_GR+=palets_bleu*point_balance;
+ //SCORE_GLOBAL=SCORE_GR+SCORE_PR;
+ //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL);
+ break;
+
+ case NB_PALETS_VERTS://nb de palets vert/rouge mis dans l'accelerateur
+ unsigned short palets_verts=(unsigned short)msgRxBuffer[FIFO_lecture].data[0];
+ SCORE_GR+=palets_verts*point_accelerateur;
+ //SCORE_GLOBAL=SCORE_GR+SCORE_PR;
+ //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL);
+ break;
+#endif
+ case ODOMETRIE_BIG_POSITION:
+ case ODOMETRIE_SMALL_POSITION:
+
+ x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
+ y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
+ theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
+ break;
+
+ case ACK_ACTION:
+ if(waitingAckID == msgRxBuffer[FIFO_lecture].id) {
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+ }
+ break;
+
+ case BALISE_DANGER :
+ SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
+ balise_danger();
+ break;
+
+ case BALISE_STOP:
+ SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
+ balise_stop(FIFO_lecture);
+ break;
+
+ case BALISE_END_DANGER:
+ SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
+ balise_end_danger(&instruction,&dodgeq,&gameEtat,target_x_robot,target_y_robot,target_theta_robot, theta_robot,x_robot,y_robot);
+ break;
+
+ case RECEPTION_DATA:
+ telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
+ telemetreDistance= (float)telemetreDistance*100.0f*35.5f+50.0f;
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+ break;
+
+ case RECEPTION_RECALAGE:
+ wait_us(150);
+ flagReceptionTelemetres = 1;
+ // telemetreDistance_avant_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //on récupère la distance traité par l'autre micro
+ memcpy(&telemetreDistance_avant_droite,msgRxBuffer[FIFO_lecture].data,2);
+ telemetreDistance_avant_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
+ telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
+ telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
+
+
+ if(ModeDemo==1) {
+ sprintf(message,"%04dmm L:%d",telemetreDistance_avant_droite,DT_AVD_interrupt);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"LASER AVD : ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
+
+ sprintf(message1,"%04dmm L:%d",telemetreDistance_avant_gauche,DT_AVG_interrupt);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVG : ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message1, LEFT_MODE);
+
+
+ sprintf(message4,"%04d",theta_robot);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(13),(unsigned char *)"THETA: ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(13),(unsigned char *)message4, LEFT_MODE);
+
+
+ sprintf(message2,"%04dmm L:%d",telemetreDistance_arriere_gauche,DT_ARG_interrupt);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER ARG : ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message2, LEFT_MODE);
+
+ sprintf(message3,"%04dmm L:%d",telemetreDistance_arriere_droite,DT_ARD_interrupt);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
+ lcd.DisplayStringAt(0, LINE(18),(unsigned char *)"LASER ARD : ",LEFT_MODE);
+ lcd.DisplayStringAt(200, LINE(18),(unsigned char *)message3, LEFT_MODE);
+ }
+ break;
+ case RECEPTION_TELEMETRE_LOGIQUE:
+
+ DT_AVD_interrupt=msgRxBuffer[FIFO_lecture].data[0];
+ DT_AVG_interrupt=msgRxBuffer[FIFO_lecture].data[1];
+ DT_ARG_interrupt=msgRxBuffer[FIFO_lecture].data[2];
+ DT_ARD_interrupt=msgRxBuffer[FIFO_lecture].data[3];
+
+ break;
+ case RECEPTION_COULEUR:
+ if (blocage_balise==0) {
+ couleur1=msgRxBuffer[FIFO_lecture].data[0];
+ couleur2=msgRxBuffer[FIFO_lecture].data[1];
+ couleur3=msgRxBuffer[FIFO_lecture].data[2];
+
+ /*lcd.DisplayStringAt(0,LINE(16),(unsigned char *)couleur1+'0',LEFT_MODE);
+ lcd.DisplayStringAt(0,LINE(16+1),(unsigned char *)couleur2+'0',LEFT_MODE);
+ lcd.DisplayStringAt(0,LINE(16+2),(unsigned char *)couleur3+'0',LEFT_MODE);*/
+ }
+
+ break;
+ /*
+ case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
+ actual_instruction = instruction.nextLineError;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ // waitingAckID_FIN=0;
+ // waitingAckFrom_FIN=0;
+ SendRawId(0x40);
+ break;*/
+ }
+ FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+ }
+}
+
+
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: Bouton_Strat */
+/* DESCRIPTION : Sélection de la strat sur le lcd puis envoie sur CAN (à modifier!) */
+/****************************************************************************************/
+signed char Bouton_Strat (void)
+{
+ Button STRAT_1 (0, 30, 190, 110, strat_sd[0]);
+ Button STRAT_2 (210, 30, 190, 110, strat_sd[1]);
+ Button STRAT_3 (0, 150, 190, 110, strat_sd[2]);
+ Button STRAT_4 (210, 150, 190, 110, strat_sd[3]);
+ Button STRAT_5 (0, 270, 190, 110,strat_sd[4]);
+ Button STRAT_6 (210, 270, 190, 110, strat_sd[5]);
+ Button STRAT_7 (0, 390, 190, 110, strat_sd[6]);
+ Button STRAT_8 (210, 390, 190, 110, strat_sd[7]);
+ Button STRAT_9 (0, 510, 190, 110, strat_sd[8]);
+ Button STRAT_10 (210, 510, 190, 110, strat_sd[9]);
+ Button RETOUR (0, 680, 400, 110, "--Precedent--");
+ //Definition des boutons
+
+ Ack_strat = 0;
+ Strat = 0;
+ STRAT_1.Draw(0xFFF0F0F0, 0);
+ STRAT_2.Draw(0xFFF0F0F0, 0);
+ STRAT_3.Draw(0xFFF0F0F0, 0);
+ STRAT_4.Draw(0xFFF0F0F0, 0);
+ STRAT_5.Draw(0xFFF0F0F0, 0);
+ STRAT_6.Draw(0xFFF0F0F0, 0);
+ STRAT_7.Draw(0xFFF0F0F0, 0);
+ STRAT_8.Draw(0xFFF0F0F0, 0);
+ STRAT_9.Draw(0xFFF0F0F0, 0);
+ STRAT_10.Draw(0xFFF0F0F0, 0);
+ RETOUR.Draw(0xFFFF0000, 0);
+
+ while(Ack_strat == 0) {
+ canProcessRx();
+ CANMessage msgTx=CANMessage();
+ //msgTx.id=ECRAN_CHOICE_STRAT;
+ if (RETOUR.Touched())
+ return -1;
+ while(RETOUR.Touched());
+ //////////////////////////////STRATEGIE N°1
+ if (STRAT_1.Touched()) {
+ Strat = 0;
+ //msgTx.data[0] = 0x1;
+ //can2.write(msgTx);
+ while(STRAT_1.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////STRATEGIE N°2
+ if (STRAT_2.Touched()) {
+ Strat = 1;
+ //msgTx.data[0] = 0x2;
+ //can2.write(msgTx);
+ while(STRAT_2.Touched());
+ Ack_strat =1;
+ }
+ //////////////////////////////STRATEGIE N°3
+ if (STRAT_3.Touched()) {
+ Strat = 2;
+ //msgTx.data[0] = 0x3;
+ //can2.write(msgTx);
+ while(STRAT_3.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////STRATEGIE N°4
+ if (STRAT_4.Touched()) {
+ Strat = 3;
+ //msgTx.data[0] = 0x4;
+ //can2.write(msgTx);
+ while(STRAT_4.Touched());
+ Ack_strat =1;
+ }
+ ///////////////////////////////STRATEGIE N°5
+ if (STRAT_5.Touched()) {
+ Strat = 4;
+ //msgTx.data[0] = 0x5;
+ //can2.write(msgTx);
+ while(STRAT_5.Touched());
+ Ack_strat =1;
+ }
+ ////////////////////////////////STRATEGIE N°6
+ if (STRAT_6.Touched()) {
+ Strat = 5;
+ //msgTx.data[0] = 0x6;
+ //can2.write(msgTx);
+ while(STRAT_6.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////////STRATEGIE N°7
+ if (STRAT_7.Touched()) {
+ Strat = 6;
+ //msgTx.data[0] = 0x7;
+ //can2.write(msgTx);
+ while(STRAT_7.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////////STRATEGIE N°8
+ if (STRAT_8.Touched()) {
+ Strat = 7;
+ //msgTx.data[0] = 0x8;
+ //can2.write(msgTx);
+ while(STRAT_8.Touched());
+ Ack_strat =1;
+ }
+ /////////////////////////////////STRATEGIE N°9
+ if (STRAT_9.Touched()) {
+ Strat = 8;
+ //msgTx.data[0] = 0x9;
+ //can2.write(msgTx);
+ while(STRAT_9.Touched());
+ Ack_strat =1;
+ }
+ ///////////////////////////////////STRATEGIE N°10
+ if (STRAT_10.Touched()) {
+ Strat = 9;
+ //msgTx.data[0] = 0xA;
+ //can2.write(msgTx);
+ while(STRAT_10.Touched());
+ Ack_strat =1;
+ }
+
+ }
+ return Strat;
+
+}
+
+void affichage_compteur (int nombre)
+{
+ int dizaine=0,unite=0,centaine=0;
+ centaine = nombre/100;
+ dizaine = nombre/10;
+ unite = nombre-(10*dizaine);
+ print_segment(unite,-50);
+ print_segment(dizaine,100);
+ if(centaine!=0) {
+ print_segment(centaine,350);
+ }
+
+}
+
+
+//****print_segment***
+//Dessine en 7 segment le nombre en parametre
+// A
+// =====
+// | |
+// B | G | E
+// |=====|
+// C | | F
+// | |
+// =====
+// D
+/*
+position pour le chiffre des unites
+lcd.FillRect(460,75,120,25);// A
+lcd.FillRect(435,100,25,120);// B
+lcd.FillRect(435,245,25,120);// C
+lcd.FillRect(460,365,120,25);// D
+lcd.FillRect(580,100,25,120);// E
+lcd.FillRect(580,245,25,120);// F
+lcd.FillRect(460,220,120,25);// G
+
+position pour le chiffre des dizaines
+lcd.FillRect(260,75,120,25);// A
+lcd.FillRect(235,100,25,120);// B
+lcd.FillRect(235,245,25,120);// C
+lcd.FillRect(260,365,120,25);// D
+lcd.FillRect(380,100,25,120);// E
+lcd.FillRect(380,245,25,120);// F
+lcd.FillRect(260,220,120,25);// G
+*/
+
+void print_segment(int nombre, int decalage)
+{
+
+ switch(nombre) {
+ case 0:
+ lcd.FillRect(240-decalage,75,120,25);
+ lcd.FillRect(215-decalage,100,25,120);
+ lcd.FillRect(215-decalage,245,25,120);
+ lcd.FillRect(360-decalage,245,25,120);
+ lcd.FillRect(360-decalage,100,25,120);
+ lcd.FillRect(240-decalage,365,120,25);
+ break;
+
+ case 1:
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(360-decalage,245,25,120);// F
+ break;
+
+ case 2:
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(215-decalage,245,25,120);// C
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+
+ case 3:
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(240-decalage,220,120,25);// G
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,245,25,120);// F
+ break;
+
+ case 4:
+ lcd.FillRect(215-decalage,100,25,120);// B
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(360-decalage,245,25,120);// F
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+
+ case 5:
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(215-decalage,100,25,120);// B
+ lcd.FillRect(240-decalage,220,120,25);// G
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,245,25,120);// F
+ break;
+
+ case 6:
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(215-decalage,100,25,120);// B
+ lcd.FillRect(215-decalage,245,25,120);// C
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,245,25,120);// F
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+
+ case 7:
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(360-decalage,245,25,120);// F
+ break;
+
+ case 8:
+ lcd.FillRect(240-decalage,75,120,25); // A
+ lcd.FillRect(215-decalage,100,25,120);
+ lcd.FillRect(215-decalage,245,25,120);
+ lcd.FillRect(360-decalage,245,25,120);//...
+ lcd.FillRect(360-decalage,100,25,120);
+ lcd.FillRect(240-decalage,365,120,25);
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+
+ case 9:
+ lcd.FillRect(240-decalage,75,120,25);// A
+ lcd.FillRect(215-decalage,100,25,120);// B
+ lcd.FillRect(240-decalage,365,120,25);// D
+ lcd.FillRect(360-decalage,100,25,120);// E
+ lcd.FillRect(360-decalage,245,25,120);// F
+ lcd.FillRect(240-decalage,220,120,25);// G
+ break;
+ }
+}
+
+void effacer_segment(long couleur)
+{
+ lcd.SetTextColor(couleur);
+ lcd.FillRect(240-200,75,120,25); // A
+ lcd.FillRect(215-200,100,25,120);
+ lcd.FillRect(215-200,245,25,120);
+ lcd.FillRect(360-200,245,25,120);//...
+ lcd.FillRect(360-200,100,25,120);
+ lcd.FillRect(240-200,365,120,25);
+ lcd.FillRect(240-200,220,120,25);// G
+
+ lcd.FillRect(240,75,120,25); // A
+ lcd.FillRect(215,100,25,120);
+ lcd.FillRect(215,245,25,120);
+ lcd.FillRect(360,245,25,120);//...
+ lcd.FillRect(360,100,25,120);
+ lcd.FillRect(240,365,120,25);
+ lcd.FillRect(240,220,120,25);// G
+}
+
+short recalageAngulaireCapteur(void)
+{
+ unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
+ unsigned int moyennageTelemetre = 0;
+ unsigned short angleAvant = 0;
+ unsigned short angleArriere = 0;
+ unsigned short orientationArrondie = 0;
+
+ unsigned short position_avant_gauche=0;
+ unsigned short position_avant_droite=0;
+ unsigned short position_arriere_gauche=0;
+ unsigned short position_arriere_droite=0;
+
+ unsigned short tempo= telemetreDistance_arriere_gauche;
+ telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
+ telemetreDistance_arriere_droite=tempo;
+
+
+
+ if(theta_robot >= 450 && theta_robot <= 1350)
+ orientationArrondie = 90;
+ else if(theta_robot <= -450 && theta_robot >= -1350)
+ orientationArrondie = 270;
+ else if(theta_robot <= 450 && theta_robot >= -450)
+ orientationArrondie = 0;
+ else if(theta_robot >= 1350 && theta_robot <= -1350)
+ orientationArrondie = 180;
+
+ // Calcul de position pour faire la vérification de cohérence
+ if(orientationArrondie == 90 || orientationArrondie == 270) {
+ position_avant_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
+ position_avant_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
+ position_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_gauche;
+ position_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_droite;
+
+ } else if(orientationArrondie == 0 || orientationArrondie == 180) {
+ position_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
+ position_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
+ position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
+ position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
+ }
+
+
+ if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
+ if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
+ if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1) {
+ if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
+ angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double)ESPACE_INTER_TELEMETRE ))/M_PI;
+ else
+ angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleArriere;
+ }
+ }
+ } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
+ if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
+ if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1) {
+ if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
+ angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double) ESPACE_INTER_TELEMETRE ))/M_PI;
+ else
+ angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleArriere;
+ }
+ }
+ }
+
+ if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
+ if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
+ if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1) {
+ if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
+ else
+ angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleAvant;
+ }
+ }
+ } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
+ if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
+ if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1) {
+ if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
+ else
+ angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleAvant;
+ }
+ }
+ }
+
+ angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;
+
+ if(nombresDeMesuresAuxTelemetresQuiSontCoherentes) {
+ if(orientationArrondie == 0) {
+ angleRecalage -= 900;
+
+ /*if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ distanceRecalage = *);
+ else
+ distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/
+ } else if(orientationArrondie == 90) {
+ angleRecalage += 0;
+ } else if(orientationArrondie == 180) {
+ angleRecalage += 900;
+ } else if(orientationArrondie == 270) {
+ angleRecalage += 1800;
+ }
+ }
+
+ return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot;
+}
+
+short recalageDistanceX(void)
+{
+ unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
+ unsigned int moyennageTelemetre = 0;
+
+ unsigned short tempo= telemetreDistance_arriere_gauche;
+ telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
+ telemetreDistance_arriere_droite=tempo;
+
+ telemetreDistance_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
+ telemetreDistance_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
+ telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
+ telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
+
+ if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1) {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_gauche;
+ }
+ if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_droite;
+ }
+ if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1) {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_gauche;
+ }
+ if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_droite;
+ }
+
+ moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
+
+ return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : x_robot; //SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
+}
+
+short recalageDistanceY(void)
+{
+ unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
+ unsigned int moyennageTelemetre = 0;
+
+ unsigned short tempo= telemetreDistance_arriere_gauche;
+ telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
+ telemetreDistance_arriere_droite=tempo;
+
+ telemetreDistance_avant_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
+ telemetreDistance_avant_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
+ telemetreDistance_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_gauche;
+ telemetreDistance_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_droite;
+
+ if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1) {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_gauche;
+ }
+ if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1) {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_avant_droite;
+ }
+ if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1) {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_gauche;
+ }
+ if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1) {
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += telemetreDistance_arriere_droite;
+ }
+
+ moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
+
+ return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Strategie/Strategie.h Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,81 @@
+#ifndef CRAC_STRATEGIE
+#define CRAC_STRATEGIE
+
+// codeur droit A maron bleu, B maron
+typedef enum
+{
+ RECALAGE_1,
+ RECULER_1,
+ TOURNER,
+ RECALAGE_2,
+ RECULER_2,
+ GOTOPOS,
+ FIN_POS,
+}E_Stratposdebut;
+
+typedef enum
+{
+ ETAT_CHECK_CARTE_SCREEN, //Envoie check carte screen
+ ETAT_CHECK_CARTE_SCREEN_WAIT_ACK, //Time out de 1s si erreur clignotement des led et fin prog
+ ETAT_CHECK_CARTES, //Envoie check toutes les carte
+ ETAT_CHECK_CARTES_WAIT_ACK, //Time out de 1s
+ ETAT_WAIT_FORCE,//Attente du forçage du lancement
+ ETAT_CONFIG, //attente reception du choix du mode( debug ou game)
+
+ ETAT_GAME_INIT,//Mise en mémoire du fichier de stratégie
+ ETAT_GAME_WAIT_FOR_JACK,//Attente du retrait du jack
+ ETAT_GAME_START,//Lancement du timer 90s
+ ETAT_GAME_LOAD_NEXT_INSTRUCTION,
+ ETAT_GAME_PROCESS_INSTRUCTION,
+ ETAT_GAME_WAIT_ACK,
+ ETAT_GAME_JUMP_TIME,
+ ETAT_GAME_JUMP_CONFIG,
+ ETAT_GAME_JUMP_POSITION,
+ ETAT_GAME_WAIT_END_INSTRUCTION,
+
+ /* A CHIER SERIEUX C NAZE
+
+ ETAT_WARNING_TIMEOUT,//Attente de la trame fin de danger ou du timeout de 2s
+ ETAT_WARING_END_BALISE_WAIT,//Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
+ ETAT_WARNING_END_LAST_INSTRUCTION,//trouver le meilleur moyen de reprendre l'instruction en cours
+ ETAT_WARNING_SWITCH_STRATEGIE,//Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
+
+ ETAT_TELEMETRE_BALANCE,
+ */
+
+
+ ETAT_END,
+ ETAT_END_LOOP,
+
+} E_stratGameEtat;
+
+/*
+typedef enum
+{
+ ETAT_INIT_EVITEMENT,
+ ETAT_ESTIMATION_POSITION,
+ ETAT_ESTIMATION_POSITION_ROTATION_ACK,
+ ETAT_ESTIMATION_POSITION_ROTATION_ACK_END,
+ ETAT_CALCUL_CHEMIN,
+ ETAT_FIN_EVITEMENT
+
+} E_Evitement;
+*/
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx */
+/* DESCRIPTION : Fonction de traitement des messages CAN */
+/****************************************************************************************/
+void canProcessRx(void);
+void automate_process(void);
+void automate_etat_ihm(void);
+void init_lcd(void);
+
+void affichage_debug(int var);
+
+short recalageAngulaireCapteur(void);
+short recalageDistanceX(void);
+short recalageDistanceY(void);
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Strategie/liaison_Bluetooth.lib Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/CRAC-Team/code/liaison_Bluetooth/#1c3ff1ff1bc0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Jan 30 16:48:59 2020 +0000
@@ -0,0 +1,101 @@
+#include "mbed.h"
+#include "global.h"
+#include <CAN.h>
+
+CAN can1(PB_8,PB_9); // Rx&Tx pour le CAN
+CAN can2(PB_5, PB_13);
+
+void bluetooth_init(void);
+DigitalIn choix_robot(PG_12);
+CANMessage msgRxBuffer[SIZE_FIFO];
+unsigned char FIFO_ecriture;
+char strat_sd[10][SIZE+8];
+char PATH[10][SIZE+8];
+
+Serial rn42_pr(PG_14, PG_9);
+//Serial rn42(PA_1,PA_2);
+Serial rn42_Tx(PA_2,NC);
+Serial rn42_Rx(NC,PA_1);
+
+Serial pc(USBTX,USBRX);
+LiaisonBluetooth liaison_Rx(&rn42_Rx,&pc);
+LiaisonBluetooth liaison_Tx(&rn42_Tx,&pc);
+LiaisonBluetooth liaison_pr(&rn42_pr,&pc);
+
+char cheminFileStart[SIZE+8]; //Le chemin du fichier de strat, utiliser strcpy(cheminFileStart,"/local/strat.txt");
+struct S_Instruction strat_instructions[SIZE_BUFFER_FILE]; //La liste des instruction chargé en mémoire
+unsigned char nb_instructions; //Le nombre d'instruction dans le fichier de strategie
+unsigned char actual_instruction;//La ligne de l'instruction en cours d'execution
+
+
+
+unsigned char InversStrat = 1;//Si à 1, indique que l'on part de l'autre cote de la table(inversion des Y)
+
+
+unsigned short waitingAckID=0;//L'id du ack attendu
+unsigned short waitingAckFrom=0;//La provenance du ack attendu
+unsigned short waitingId=0;
+char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre
+
+DigitalOut led1(LED1);//Led d'indication de problème, si elle clignote, c'est pas bon
+DigitalOut led2(LED2);//Led d'indication de problème, si elle clignote, c'est pas bon
+DigitalOut led3(LED3);//Led d'indication de problème, si elle clignote, c'est pas bon
+DigitalOut led4(LED4);//Led d'indication de problème, si elle clignote, c'est pas bon
+
+/****************************************************************************************/
+/* FUNCTION NAME: canRx_ISR */
+/* DESCRIPTION : Interruption en réception sur le CAN */
+/****************************************************************************************/
+void canRx_ISR (void)
+{
+ if (can2.read(msgRxBuffer[FIFO_ecriture])) {
+ //if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
+ /*else*/ FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+ //canProcessRx();
+ }
+}
+
+
+
+
+/**********************************************************************************/
+/* FUNCTION NAME: main */
+/* DESCRIPTION : Fonction principal du programme */
+/**********************************************************************************/
+int main() {
+ can1.frequency(1000000); // fréquence de travail 1Mbit/s
+ can2.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
+ can2.frequency(1000000);
+#ifdef ROBOT_BIG
+ lcd.DisplayStringAt(0, 0,(uint8_t *)"Initialisation gros robot", LEFT_MODE);
+#else
+ lcd.DisplayStringAt(0, 0,(uint8_t *)"Initialisation petit robot", LEFT_MODE);
+#endif
+ led1 = 1;
+ bluetooth_init();
+ 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) {
+ automate_etat_ihm();
+ automate_process();//Boucle dans l'automate principal
+ canProcessRx();
+ }
+}
+
+void bluetooth_init(void){
+ rn42_Tx.baud(115200);
+ rn42_Rx.baud(115200);
+ rn42_pr.baud(115200);
+ pc.baud(115200);
+ pc.printf("ok1");
+ /*while(1){ // sert au paramètrage des module RN42
+ while(pc.readable()){
+ rn42_Tx.putc(pc.getc());
+
+ }
+ while(rn42_Rx.readable()){
+ pc.putc(rn42_Rx.getc());
+ }
+ }*/
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/e7ca05fa8600 \ No newline at end of file