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:
- 1:7e925468f9d9
- Child:
- 10:1964bb91b925
--- /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);
+}