CRAC Team
carte_strategie_2019
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 35:742dc6b200b0
- Parent:
- 34:6aa4b46b102e
- Child:
- 36:6dd30780bd8e
--- a/Strategie/Strategie.cpp Thu Apr 26 20:14:18 2018 +0000
+++ b/Strategie/Strategie.cpp Tue May 01 13:25:42 2018 +0000
@@ -1,4 +1,4 @@
-#include "global.h"
+ #include "global.h"
#include <string.h>
#include <sstream>
//#include "StrategieManager.h"
@@ -6,9 +6,9 @@
#define M_PI 3.14159265358979323846
-#define VERT 0xFF1467E5
+#define VERT 0xFF00FF00
#define ROUGE 0xFFFF0000
-#define BLEU 0xFF00FF00
+#define BLEU 0xFF0000FF
#define JAUNE 0xFFFEFE00
#define BLANC 0xFF000000
#define ORANGE 0xFFFFA500
@@ -80,9 +80,10 @@
//unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
int flagSendCan=1;
unsigned char Cote = 0; //0 -> VERT | 1 -> jaune
-
+unsigned short angleRecalage = 0;
unsigned char checkCurrent = 0;
unsigned char countAliveCard = 0;
+unsigned char ligne=0;
signed char Strategie = 0; //N° de la strategie (1-10)
@@ -284,6 +285,16 @@
flagSendCan = 1;
}
+void affichage_var(double Var){
+ if(ligne==7)
+ ligne=0;
+ char aff[10]="toto";
+ sprintf(aff,"%lf ",Var);
+ lcd.DisplayStringAt(120, LINE(5+(ligne)), (uint8_t *)aff, LEFT_MODE);
+ ligne++;
+
+}
+
void affichage_debug(int Var){
int i;
int conv=(int)Var;
@@ -952,25 +963,25 @@
cptf=gameTimer.read();
lcd.SetTextColor(LCD_COLOR_BLACK);
cpt=int(cptf);
- if(cpt != cpt1){
+ /*if(cpt != cpt1){
lcd.Clear(VERT);
affichage_compteur(100-cpt);
- }
+ }*/
cpt1=cpt;
flag_timer=0;
//affichage_debug(gameEtat);
lcd.SetBackColor(LCD_COLOR_WHITE);
- char position_x[10]="toto";
+ /*char position_x[10]="toto";
char position_y[10]="toto";
char position_theta[10]="toto";
sprintf(position_x,"%d ",x_robot);
sprintf(position_y,"%d",y_robot);
- sprintf(position_theta,"%d ",theta_robot);
+ sprintf(position_theta,"%d ",angleRecalage);
lcd.DisplayStringAt(120, LINE(18), (uint8_t *)position_x, LEFT_MODE);
lcd.DisplayStringAt(120, LINE(20), (uint8_t *)position_y, LEFT_MODE);
- lcd.DisplayStringAt(120, LINE(22), (uint8_t *)position_theta, LEFT_MODE);
+ lcd.DisplayStringAt(120, LINE(22), (uint8_t *)position_theta, LEFT_MODE);*/
break;
@@ -1323,6 +1334,7 @@
break;
case MV_RECALAGE:
+ wait(0.2);
if(instruction.nextActionType == MECANIQUE)
{
instruction.nextActionType = WAIT;
@@ -1347,7 +1359,7 @@
}
else //CAPTEUR
{
- unsigned char nombresDeMesuresAuxTelemtresQuiSontCoherentes = 0;
+ unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
unsigned int moyennageTelemetre = 0;
unsigned short angleAvant = 0;
unsigned short angleArriere = 0;
@@ -1358,7 +1370,7 @@
unsigned short position_arriere_gauche=0;
unsigned short position_arriere_droite=0;
- unsigned short angleRecalage = 0;
+
unsigned short distanceRecalage = 0;
SendRawId(DATA_RECALAGE);
@@ -1368,74 +1380,77 @@
// 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)
{
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))?3000:0) + (((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))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_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)
{
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_gauche;
}
if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1)
{
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_droite;
}
if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1)
{
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_gauche;
}
if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1)
{
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_droite;
}
- moyennageTelemetre /= nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+ moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
- if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+ if(nombresDeMesuresAuxTelemetresQuiSontCoherentes)
SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
}
else if(instruction.precision == RECALAGE_X)
{
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)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_gauche;
- telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_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)
{
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_gauche;
}
if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1)
{
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_avant_droite;
}
if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
{
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_gauche;
}
if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1)
{
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += telemetreDistance_arriere_droite;
}
- moyennageTelemetre /= nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+ moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
- if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+ if(nombresDeMesuresAuxTelemetresQuiSontCoherentes)
SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
+
+
}
- /*else if(instruction.precision == RECALAGE_T)
+ else if(instruction.precision == RECALAGE_T)
{
// On arrondie l'angle pour simplifier les conditions
if(theta_robot >= 450 && theta_robot <= 1350)
@@ -1452,16 +1467,16 @@
{
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))?3000:0) + (((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))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_arriere_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)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_gauche;
- position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_arriere_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;
}
@@ -1472,22 +1487,28 @@
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 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) , (double)ESPACE_INTER_TELEMETRE ))/M_PI;
else
- angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ 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(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
+ if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
{
- if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1)
+ 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 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) , (double) ESPACE_INTER_TELEMETRE ))/M_PI;
else
- angleArriere = 900 + (1800 * atan( (double)( (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
+
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleArriere;
}
}
}
@@ -1499,44 +1520,46 @@
if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1)
{
if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
- angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
else
- angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite) ,(double) ESPACE_INTER_TELEMETRE ))/M_PI;
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
moyennageTelemetre += angleAvant;
}
}
}
else if(orientationArrondie == 0 || orientationArrondie == 180) // Si il est en axe X
{
- if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
+ if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) // Et que les mesures sont cohérentes
{
- if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1)
+ if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1)
{
if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
- angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
else
- angleAvant = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;
+ angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite) ,(double) ESPACE_INTER_TELEMETRE ))/M_PI;
- nombresDeMesuresAuxTelemtresQuiSontCoherentes++;
- moyennageTelemetre += angleArriere;
+ nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
+ moyennageTelemetre += angleAvant;
}
}
}
- angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemtresQuiSontCoherentes;
+ angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;
+
- if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
+
+ if(nombresDeMesuresAuxTelemetresQuiSontCoherentes)
{
if(orientationArrondie == 0)
{
- angleRecalage -= 90;
+ angleRecalage -= 900;
- if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
+ /*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;
+ distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/
}
else if(orientationArrondie == 90)
{
@@ -1544,18 +1567,31 @@
}
else if(orientationArrondie == 180)
{
- angleRecalage += 90;
+ angleRecalage += 900;
}
else if(orientationArrondie == 270)
{
- angleRecalage += 180;
+ angleRecalage += 1800;
}
}
- if(nombresDeMesuresAuxTelemtresQuiSontCoherentes)
- SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, moyennageTelemetre);
- } */
+ lcd.Clear(VERT);
+ affichage_var(nombresDeMesuresAuxTelemetresQuiSontCoherentes);
+ affichage_var(orientationArrondie);
+ affichage_var(angleAvant);
+ affichage_var(angleArriere);
+ affichage_var(angleRecalage);
+ affichage_var(telemetreDistance_avant_gauche);
+ affichage_var(telemetreDistance_avant_droite);
+
+ if(nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)){
+ SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, angleRecalage);
+ lcd.DisplayStringAt(120, LINE(18),(uint8_t *) "recalement theta", LEFT_MODE);
+ }
+
+
+ }
}
break;
@@ -1567,7 +1603,6 @@
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;
}
@@ -1757,7 +1792,7 @@
break;
case ETAT_GAME_WAIT_END_INSTRUCTION:
canProcessRx();
- if(instruction.order==MV_XYT && (waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0)){
+ /*if(instruction.order==MV_XYT && (waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0)){
if((x_robot<=target_x_robot-2 || x_robot>=target_x_robot+2)||(y_robot<=target_y_robot-2 || y_robot>=target_y_robot+2)||(theta_robot<=target_theta_robot-2 || theta_robot>=target_theta_robot+2)){
gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
}
@@ -1765,8 +1800,8 @@
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
}
- }
- else if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée
+ } */
+ 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
}
@@ -2107,6 +2142,13 @@
couleur3=msgRxBuffer[FIFO_lecture].data[2];
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;
}