CRAC Team
carte_strategie_2019
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 67:96f914f92d2d
- Parent:
- 63:bcfe62e3f1d2
- Child:
- 72:5b1b3e151e59
--- a/Strategie/Strategie.cpp Sat May 25 16:10:21 2019 +0000
+++ b/Strategie/Strategie.cpp Sun May 26 12:49:15 2019 +0000
@@ -2,6 +2,7 @@
#include <string.h>
#include <sstream>
#include <math.h>
+#include <vector>
//#include "StrategieManager.h"
@@ -496,200 +497,7 @@
}
}
break;
- /* ///////////////////////////////TESTE LES SERVOS LIES AU TRI DES BALLES///////////////////////////////
- case DEMO_TRIEUR:
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
- TRI.Draw(VERT, 0);
- AIGUILLEUR_D.Draw(VERT, 0);
- AIGUILLEUR_G.Draw(VERT, 0);
- AIGUILLEUR_CTRE.Draw(VERT, 0);
- while(etat==DEMO_TRIEUR) {
- if(RETOUR.Touched()) {
- while (RETOUR.Touched());
- etat=DEMO;
- } else if(TRI.Touched()) {
- while (TRI.Touched());
- SendRawId(AIGUILLEUR_CENTRE);
- wait(0.5);
- SendRawId(AIGUILLEUR_DROITE);
- wait(0.5);
- SendRawId(AIGUILLEUR_GAUCHE);
- wait(0.5);
- SendRawId(AIGUILLEUR_CENTRE);
-
- break;
- } else if(AIGUILLEUR_D.Touched()) {
- while (AIGUILLEUR_D.Touched());
- SendRawId(AIGUILLEUR_DROITE);
- break;
- } else if(AIGUILLEUR_G.Touched()) {
- while (AIGUILLEUR_G.Touched());
- SendRawId(AIGUILLEUR_GAUCHE);
- break;
-
- } else if(BRAS_ABEILLE_OFF.Touched()) {
- while (BRAS_ABEILLE_OFF.Touched());
- SendRawId(BRAS_ABEILLE_DOWN);
- break;
- } else if(AIGUILLEUR_CTRE.Touched()) {
- while (AIGUILLEUR_CTRE.Touched());
- SendRawId(AIGUILLEUR_CENTRE);
- break;
- }
-
- }
- break;
- case DEMO_IMMEUBLE: //TESTE LE MONTE IMMEUBLE SUIVANT UN CODE COULEUR CHOISI
- int color=0;
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"Choix du code couleur", LEFT_MODE);
-
- CANMessage msgTx=CANMessage();
- msgTx.id=MONTER_IMMEUBLE; // Monter immeuble
- msgTx.len=3;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
-
-
- while(etat==DEMO_IMMEUBLE) {
- switch(color) {
- case 0:
-
- RETOUR.Draw(ROUGE,0);
- COLOR_NOIR.Draw(NOIR,1);
- COLOR_ORANGE.Draw(ORANGE,0);
- COLOR_JAUNE.Draw(JAUNE,0);
- COLOR_VERT.Draw(VERT,0);
- COLOR_BLEU.Draw(BLEU,0);
-
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.SetTextColor(NOIR);
- lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 1", LEFT_MODE);
- while(color==0) {
- if(COLOR_ORANGE.Touched()) {
- while(COLOR_ORANGE.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
- color++;
- } else if (COLOR_NOIR.Touched()) {
- while(COLOR_NOIR.Touched());
- COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
- color++;
- } else if (COLOR_VERT.Touched()) {
- while(COLOR_VERT.Touched());
- COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
- msgTx.data[color]=3;
- color++;
- } else if (COLOR_JAUNE.Touched()) {
- while(COLOR_JAUNE.Touched());
- COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=4;
- color++;
- } else if (COLOR_BLEU.Touched()) {
- while(COLOR_BLEU.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=5;
- color++;
- }
- }
- break;
-
- case 1:
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.SetTextColor(NOIR);
- lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 2", LEFT_MODE);
- if(COLOR_ORANGE.Touched()) {
- while(COLOR_ORANGE.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
- color++;
- } else if (COLOR_NOIR.Touched()) {
- while(COLOR_NOIR.Touched());
- COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
- color++;
- } else if (COLOR_VERT.Touched()) {
- while(COLOR_VERT.Touched());
- COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
- msgTx.data[color]=3;
- color++;
- } else if (COLOR_JAUNE.Touched()) {
- while(COLOR_JAUNE.Touched());
- COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=4;
- color++;
- } else if (COLOR_BLEU.Touched()) {
- while(COLOR_BLEU.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=5;
- color++;
- }
- break;
-
- case 2:
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.SetTextColor(NOIR);
- lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 3", LEFT_MODE);
- if(COLOR_ORANGE.Touched()) {
- while(COLOR_ORANGE.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=1;
- color++;
- } else if (COLOR_NOIR.Touched()) {
- while(COLOR_NOIR.Touched());
- COLOR_NOIR.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=2;
- color++;
- } else if (COLOR_VERT.Touched()) {
- while(COLOR_VERT.Touched());
- COLOR_VERT.Draw(LCD_COLOR_WHITE);
-
- msgTx.data[color]=3;
- color++;
- } else if (COLOR_JAUNE.Touched()) {
- while(COLOR_JAUNE.Touched());
- COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=4;
- color++;
- } else if (COLOR_BLEU.Touched()) {
- while(COLOR_BLEU.Touched());
- COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
- msgTx.data[color]=5;
- color++;
- }
- break;
- case 3:
- lcd.Clear(LCD_COLOR_WHITE);
- lcd.SetBackColor(LCD_COLOR_WHITE);
- lcd.SetTextColor(NOIR);
-
- lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"Immeuble en construction", LEFT_MODE);
- RETOUR.Draw(ROUGE,0);
- can2.write(msgTx);
- color++;
- break;
-
- case 4:
- if(RETOUR.Touched()) {
- while(RETOUR.Touched());
- etat=DEMO;
- }
- break;
- }
- if(RETOUR.Touched()) {
- while(RETOUR.Touched());
- etat=DEMO;
- }
- }
- break;
-
-
-
- */
+
case TEST_SERVO: //TEST DU RESTE DES SERVOS DISPO HORS TIR
lcd.SetBackColor(LCD_COLOR_WHITE);
@@ -743,37 +551,8 @@
LANCEUR_OFF.Draw(VERT, 0);
RETOUR.Draw(ROUGE, 0);
- etat=DEMO;/*
- while(etat==TEST_TIR) {
- if(TIR_CHATEAU.Touched()) {
- while (TIR_CHATEAU.Touched());
- SendRawId(INCLINAISON_CHATEAU);
- break;
- } else if (EPURATION.Touched()) {
- while (EPURATION.Touched());
- SendRawId(INCLINAISON_EPURATION);
- break;
- } else if(LANCEUR_ON.Touched()) {
- while (LANCEUR_ON.Touched());
- CANMessage msgTx=CANMessage();
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- msgTx.id=LANCEMENT_MOTEUR_TIR_ON;
-
- msgTx.len=1;
- msgTx.data[0]=0;
- can2.write(msgTx);
- break;
- } else if(LANCEUR_OFF.Touched()) {
- while (LANCEUR_OFF.Touched());
- SendRawId(LANCEMENT_MOTEUR_TIR_OFF);
- break;
- } else if (RETOUR.Touched()) {
- while (RETOUR.Touched());
- etat=DEMO;
-
- }
- }*/
+ etat=DEMO;
+
break;
@@ -1276,6 +1055,80 @@
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
actionPrecedente = MV_COURBURE;
waitingAckID = ASSERVISSEMENT_COURBURE;
@@ -1442,6 +1295,8 @@
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) {
@@ -1524,6 +1379,10 @@
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;
@@ -2215,9 +2074,6 @@
float seuil_bas_arriere = 5.0;
float seuil_haut_arriere = 7.0;
#endif
-
-
-
if (instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
|| instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
|| instruction.order == MV_COURBURE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
@@ -2225,7 +2081,6 @@
|| instruction.order == MV_XYT && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
|| instruction.order == MV_XYT && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere ) { //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
-
if(needToStop() != 0 && ingnorBaliseOnce ==0 && ingnorBalise==0) {
if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) {
SendRawId(ASSERVISSEMENT_STOP);
@@ -2240,7 +2095,6 @@
timeoutWarning.reset();
timeoutWarning.start();//Reset du timer utiliser par le timeout
}
- //stop_evitement=1;
gameEtat = ETAT_WARNING_TIMEOUT;
}
}
@@ -2257,21 +2111,6 @@
}
break;
- /*case OBJET_SUR_TABLE:
- if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
-
- gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
- }
- else{
-
- waitingAckFrom = 0;
- waitingAckID = 0;
-
- strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2);
- strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2);
- }
- modeTelemetre = 0;
- break;*/
case RECEPTION_DATA:
telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
@@ -2644,7 +2483,13 @@
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)