CRAC Team
code de la carte IHM avant les bugs et avant le travail effectué avec Melchior
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Diff: Evitement/Evitement.cpp
- Revision:
- 37:f1a8734c193d
- Parent:
- 21:d137ec53c3a9
--- a/Evitement/Evitement.cpp Mon May 31 13:36:03 2021 +0000
+++ b/Evitement/Evitement.cpp Sat Jul 17 16:07:29 2021 +0000
@@ -25,10 +25,52 @@
/* FUNCTION NAME: Balise Danger */
/* DESCRIPTION : FIFO -> BALISE_DANGER */
/****************************************************************************************/
-unsigned short balise_danger(void){
- SendSpeed(150);
- return(0);
+
+unsigned short balise_danger(signed char FIFO_lecture)
+{
+ /*signed char fin_angle_detection;
+ signed char debut_angle_detection;
+ float angle_moyen_balise_IR = 0.0;
+// Debug_Audio(3,2);
+ //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;
+ char seuil_bas_arriere = 5;
+ char seuil_haut_arriere = 7;
+ char seuil_bas_avant = 0xF;
+ char seuil_haut_avant = 0xD;
+ // #endif
+ // LARNAUDIE 3/7/2021 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))
+ if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
+ {
+ if(asser_stop_direction==1)
+ {
+ if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+ ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ }
+ else
+ {
+ if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+ ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ }
+ }
+ return(0);*/
}
+
/****************************************************************************************/
/* FUNCTION NAME: Balise Stop */
/* DESCRIPTION : FIFO -> BALISE_STOP */
@@ -38,9 +80,9 @@
signed char fin_angle_detection;
signed char debut_angle_detection;
float angle_moyen_balise_IR = 0.0;
- Debug_Audio(3,2);
+// Debug_Audio(3,2);
//on recupere l'info d'angle de detection--------------------------------------
- if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte
+ 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
@@ -54,57 +96,108 @@
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);
+ char seuil_bas_arriere = 5;
+ char seuil_haut_arriere = 7;
+ char seuil_bas_avant = 0xF;
+ char seuil_haut_avant = 0xD;
+ // #endif
+ // LARNAUDIE 3/7/2021 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))
+ if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT))
+ {
+ if(Cote == 0)//Jaune
+ {
+ if((x_robot >= 1600 && y_robot >= 2200) || (y_robot >= 2500) || (x_robot >= 1750 && y_robot >= 1000 && y_robot <= 1400)) {}
+ else
+ {
+ if(asser_stop_direction==1)
+ {
+ if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+ ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ }
+ else
+ {
+ if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+ ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ }
+ }
+ }
+
+ else if(Cote == 1)//Bleu
+ {
+ if((x_robot >= 1600 && y_robot <= 800) || (y_robot <= 500) || (x_robot >= 1750 && y_robot >= 1600 && y_robot <= 2000)) {}
+ else
+ {
+ if(asser_stop_direction==1)
+ {
+ if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant))
+ ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) )
+ SendRawId(ASSERVISSEMENT_STOP);
+ }
+ else
+ {
+ if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere))
+ ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=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, 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){
- Debug_Audio(3,1);
+unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short local_target_x_robot, signed short local_target_y_robot, signed short local_target_theta_robot, signed short local_theta_robot,signed short x_robot,signed short y_robot){
+ ingnorInversionOnce=1;
+ wait_ms(1000);
switch(instruction->order){
+ case MV_RECALAGE:
+ gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
+ break;
+
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
+ instruction->arg1 = local_target_x_robot;// X
+ instruction->arg2 = local_target_y_robot;// Y
+ instruction->arg3 = local_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
+ instruction->arg1 = local_target_x_robot;// X
+ instruction->arg2 = local_target_y_robot;// Y
+ instruction->arg3 = local_target_theta_robot;// T
break;
case MV_XYT:
gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
break;
case MV_COURBURE:
- unsigned short alpha;
+ 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){
+ if(instruction->direction==LEFT)
+ alpha=(dodgeq->inst[0].arg3-theta_robot);
+ else
+ alpha=(theta_robot-dodgeq->inst[0].arg3);
+ if(alpha>3600)
+ alpha=alpha-3600;
+ if(alpha<-3600)
+ alpha=alpha+3600;
+ if(alpha<0)
+ alpha=-alpha;
+ 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){
+ }
+ else if(alpha<900)
+ {
dodgeq->nb=1;
instruction->arg1=dodgeq->inst[1].arg1;//x
instruction->arg2=dodgeq->inst[1].arg2;//y
@@ -132,6 +225,6 @@
}
break;
}
- SendSpeed(300);
+// SendSpeed(300);
return(0);
}