Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 51:aa6e09f2cfec
- Parent:
- 50:a5361ffeefc8
- Child:
- 52:a47350923b5e
diff -r a5361ffeefc8 -r aa6e09f2cfec Strategie/Strategie.cpp --- a/Strategie/Strategie.cpp Fri May 17 21:06:46 2019 +0000 +++ b/Strategie/Strategie.cpp Sat May 18 16:54:06 2019 +0000 @@ -1603,10 +1603,10 @@ case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s if(timeoutWarning.read_ms() >= BALISE_TIMEOUT) { //ça fait plus de 2s, il faut changer de stratégie gameEtat = ETAT_EVITEMENT; - if(Fevitement==1) { + /* if(Fevitement==1) { EvitEtat= 0; Fevitement=0; - } + }*/ /*------------------------------------- code origine @@ -1698,6 +1698,19 @@ break; case ETAT_EVITEMENT : + /* + + 90° + | + |Violet + | + | + | + | + |Jaune + | + |________________ 0° */ + char message[10]="toto"; char message1[10]="toto"; char message2[10]="toto"; @@ -1714,17 +1727,18 @@ static short cote=0; //-------------------------- static float dist_robot_adversaire=650;//distance à laquelle on s'arrete grace à la balise - int proxy=400;//distance entre point de controle et obstacle/adversaire - int proximity=400;//distance entre l'objectif et obstacle/adversaire - short taille_petit=150;// distance proxymité max mur + int proxy=300;//distance entre point de controle et obstacle/adversaire + int proximity=300;//distance entre l'objectif et obstacle/adversaire + short taille_petit=200;// distance proxymité max mur //---------------------------* static unsigned short distance=50000;//valeur impossible static unsigned short distance_prev=50000; static signed short theta_adversaire; - + switch(EvitEtat) { case 0: - + + lcd.SetBackColor(LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(2),(unsigned char *)"EVITEMENT ",LEFT_MODE); @@ -1757,59 +1771,52 @@ waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; waitingId = RECEPTION_RECALAGE; SendRawId(DATA_RECALAGE); - //wait_us(150); + wait_us(150); while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) { canProcessRx(); - // On attend que les variables soient actualise - wait_ms(1); + wait_ms(10); if(waitingId == 0) { distance=telemetreDistance_avant_droite; //on sauvegarde notre distance au robot if(distance<=distance_prev) { - /*unsigned char datatemp[2]; - memcpy(datatemp, &distance, 2); - SendMsgCan(0x750, datatemp,2); - wait_us(150);*/ distance_prev=distance; + dist_robot_adversaire=distance+100; theta_adversaire=theta_robot; - dist_robot_adversaire=distance; } waitingId = RECEPTION_RECALAGE; - wait_ms(1); SendRawId(DATA_RECALAGE); wait_us(150); } } - /* - SendSpeed(600,5000,5000);//vitesse inintiale - waitingAckID = ASSERVISSEMENT_CONFIG; - waitingAckFrom = ACKNOWLEDGE_MOTEUR; - while(waitingAckID !=0 && waitingAckFrom !=0) - canProcessRx(); - - Rotate(theta_adversaire);//Rotate(450);on revient au centre - waitingAckID = ASSERVISSEMENT_ROTATION; - waitingAckFrom = ACKNOWLEDGE_MOTEUR; - while(waitingAckID !=0 && waitingAckFrom !=0) - canProcessRx(); - - waitingAckID_FIN = ASSERVISSEMENT_ROTATION; - waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; - while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) - canProcessRx();*/ - - EvitEtat=10; + + SendSpeed(300,5000,5000);//vitesse inintiale SendSpeed(600,5000,5000) + waitingAckID = ASSERVISSEMENT_CONFIG; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + wait_us(150); + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + + /* Rotate(theta_adversaire); //on tourne a gauche pour scanner + waitingAckID = ASSERVISSEMENT_ROTATION; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + + waitingAckID_FIN = ASSERVISSEMENT_ROTATION; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) + canProcessRx();*/ + + EvitEtat=1; break; - //theta_adversaire=(theta1+theta2)/2;//version avec telemetre - //theta_adversaire=theta_robot;//version sans telemetre (robot en face case 1: short ang_target = (short)((atan2((float)(target_y_robot - y_robot), (float)(target_x_robot - x_robot)) * 1800 / M_PI) - theta_robot + 7200) % 3600; // On passe le résultat entre -1800 et 1800 - if (ang_target > 1800) ang_target = (ang_target- 3600); + if (ang_target > 1800) ang_target = (ang_target - 3600); // float dist_target = (short)sqrt((target_x_robot - x_robot)*(target_x_robot - x_robot)+(target_y_robot - y_robot)*(target_y_robot - y_robot)); - +/* float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800); float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800); @@ -1827,6 +1834,24 @@ y_cote_droit[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800); x_cote_gauche[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800); y_cote_gauche[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800); +*/ + float x_robot_adversaire = x_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)* M_PI/1800); + float y_robot_adversaire = y_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)*M_PI/1800); + + x_cote_droit[0] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800); + y_cote_droit[0] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800); + x_cote_gauche[0] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800); + y_cote_gauche[0] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800); + + x_cote_droit[1] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800); + y_cote_droit[1] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800); + x_cote_gauche[1] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800); + y_cote_gauche[1] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800); + + x_cote_droit[2] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800); + y_cote_droit[2] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800); + x_cote_gauche[2] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800); + y_cote_gauche[2] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800); SendRawId(0x0D0);//calcul @@ -1836,7 +1861,6 @@ bool cote_droit=false, cote_gauche=false; - for (int i=0; i<3; i++) { if (x_cote_droit[i]>taille_petit && x_cote_droit[i]<x_terrain-taille_petit && y_cote_droit[i] >taille_petit && y_cote_droit[i] < y_terrain-taille_petit) { cote_droit=true; @@ -1868,17 +1892,10 @@ } if ( ang_target>600 || ang_target<-600)cote=0; - - - if (!cote_droit && !cote_gauche) { - cote=0; - } - + if (!cote_droit && !cote_gauche)cote=0; //--------------------test target -------------------------------------- - if ((x_robot_adversaire >= target_x_robot-proximity && x_robot_adversaire <= target_x_robot+proximity)&&(y_robot_adversaire >= target_y_robot-proximity && y_robot_adversaire <= target_y_robot+proximity)) cote=0; - EvitEtat = 2; break; @@ -1889,10 +1906,12 @@ if(cote!=0) { for(int i=0; i<3; i++) { if(cote==-1) { - GoToPosition(y_cote_droit[i],x_cote_droit[i],theta_robot,1);// + GoToPosition(x_cote_droit[i],y_cote_droit[i],theta_robot,1); + //GoToPosition(y_cote_droit[i],x_cote_droit[i],theta_robot,1);// SendRawId(0x1D1);//evitement a gauche } else if(cote==1) { - GoToPosition(y_cote_gauche[i],x_cote_gauche[i],theta_robot,1); + GoToPosition(x_cote_gauche[i],y_cote_gauche[i],theta_robot,1); + //GoToPosition(y_cote_gauche[i],x_cote_gauche[i],theta_robot,1); SendRawId(0x2D1);//evitement a droite } waitingAckID = ASSERVISSEMENT_XYT; @@ -1927,10 +1946,10 @@ ingnorBalise=0; Fevitement=0; } - EvitEtat=2; + EvitEtat=3; break; - case 3: + case 3://on va vers l'objectif initial SendRawId(0x0D2); GoToPosition(target_x_robot,target_y_robot,target_theta_robot,1); waitingAckID = ASSERVISSEMENT_XYT; @@ -1944,7 +1963,7 @@ while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) canProcessRx(); - EvitEtat=3; + EvitEtat=4; break; case 4: //on charge l'instruction suivante et sort de l'evitement @@ -2005,6 +2024,7 @@ 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) @@ -2114,11 +2134,11 @@ -#ifdef ROBOT_BIG +//#ifdef ROBOT_BIG case ODOMETRIE_BIG_POSITION: -#else +//#else case ODOMETRIE_SMALL_POSITION: -#endif +//#endif 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); @@ -2228,7 +2248,6 @@ break; case RECEPTION_RECALAGE: - SendRawId(0x750); wait_us(150); // 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); @@ -2247,6 +2266,13 @@ 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,"%04d mm",telemetreDistance_arriere_gauche); lcd.SetBackColor(LCD_COLOR_WHITE);