![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 26:2f4fcc2354f3
- Parent:
- 25:f140c93a8666
- Child:
- 27:76ead555a63d
diff -r f140c93a8666 -r 2f4fcc2354f3 Strategie/Strategie.cpp --- a/Strategie/Strategie.cpp Fri May 26 01:27:49 2017 +0000 +++ b/Strategie/Strategie.cpp Fri May 26 04:58:47 2017 +0000 @@ -1,4 +1,5 @@ #include "Strategie.h" + #define M_PI 3.14159265358979323846 E_stratGameEtat gameEtat = ETAT_CHECK_CARTE_SCREEN; E_stratGameEtat lastEtat = ETAT_CHECK_CARTE_SCREEN; @@ -296,6 +297,7 @@ switch(instruction.order) { case MV_COURBURE://C'est un rayon de courbure + actionPrecedente = MV_COURBURE; waitingAckID = ASSERVISSEMENT_COURBURE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; if(instruction.nextActionType == ENCHAINEMENT) { @@ -316,10 +318,17 @@ localData1 = -localData1;//Inversion de la direction #ifdef ROBOT_BIG localData2 = -localData2; + localData1 = -localData1;//Inversion de la direction #endif } BendRadius(instruction.arg1, localData2, localData1, localData5); + if(instruction.direction == LEFT){ + target_theta_robot = theta_robot + localData2; + }else{ + target_theta_robot = theta_robot - localData2; + } + break; case MV_LINE://Ligne droite waitingAckID = ASSERVISSEMENT_RECALAGE; @@ -345,8 +354,8 @@ localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); GoStraight(localData2, 0, 0, localData5); - target_x_robot = x_robot + localData2*cos(theta_robot); - target_y_robot = y_robot + localData2*sin(theta_robot); + target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800); + target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800); target_theta_robot = theta_robot; break; @@ -370,6 +379,7 @@ Rotate(localData2); waitingAckID = ASSERVISSEMENT_ROTATION; waitingAckFrom = ACKNOWLEDGE_MOTEUR; + break; case MV_XYT: if(instruction.direction == BACKWARD) { @@ -652,12 +662,16 @@ break; case MV_XYT: - GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1); break; - case MV_TURN: - while(1); + case MV_COURBURE: + if(instruction.direction == LEFT){ + target_theta_robot = target_theta_robot - theta_robot; + }else{ + target_theta_robot = target_theta_robot + theta_robot; + } + BendRadius(target_theta_robot, localData2, localData1, localData5); break; } @@ -832,7 +846,7 @@ if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) { SendRawId(ASSERVISSEMENT_STOP); - while(1); // ligne à décommenter si on est en homologation + //while(1); // ligne à décommenter si on est en homologation gameEtat = ETAT_WARNING_TIMEOUT; if(gameEtat != ETAT_WARING_END_BALISE_WAIT) { timeoutWarning.reset();