Strategie_13h30
Fork of CRAC-Strat_2017_homologation_gros_rob by
Revision 22:8dec8824a6f7, committed 2017-05-25
- Comitter:
- matthieuvignon
- Date:
- Thu May 25 11:30:31 2017 +0000
- Parent:
- 21:65ba6e4d53fa
- Commit message:
- 13h30_Strategie
Changed in this revision
diff -r 65ba6e4d53fa -r 8dec8824a6f7 Globals/global.h --- a/Globals/global.h Thu May 25 06:35:42 2017 +0000 +++ b/Globals/global.h Thu May 25 11:30:31 2017 +0000 @@ -11,6 +11,7 @@ #include "Telemetre.h" #include "SerialHalfDuplex.h" #include "peripheriques.h" +#include "math.h" extern CAN can1; extern CANMessage msgRxBuffer[SIZE_FIFO];
diff -r 65ba6e4d53fa -r 8dec8824a6f7 Robots/Strategie_big.cpp --- a/Robots/Strategie_big.cpp Thu May 25 06:35:42 2017 +0000 +++ b/Robots/Strategie_big.cpp Thu May 25 11:30:31 2017 +0000 @@ -262,6 +262,8 @@ can1.write(msgTx); wait_ms(1); can1.write(msgTx); + waitingAckID= 0; + waitingAckFrom = 0; break; case 116: // pompe avant OFF @@ -281,6 +283,8 @@ can1.write(msgTx); wait_ms(1); can1.write(msgTx); + waitingAckID= 0; + waitingAckFrom = 0; break; case 120: // repos bras arrière @@ -364,7 +368,7 @@ } msgTx.data[0]=localData; - msgTx.data[1]=speed; + msgTx.data[1]=localData2; can1.write(msgTx); break; @@ -403,6 +407,9 @@ can1.write(msgTx); wait_ms(1); can1.write(msgTx); + + waitingAckID= 0; + waitingAckFrom = 0; break; case 126: // pompe arriere pwm OFF @@ -422,6 +429,8 @@ can1.write(msgTx); wait_ms(1); can1.write(msgTx); + waitingAckID= 0; + waitingAckFrom = 0; break; case 130: // baiser bras turbine
diff -r 65ba6e4d53fa -r 8dec8824a6f7 Strategie/Strategie.cpp --- a/Strategie/Strategie.cpp Thu May 25 06:35:42 2017 +0000 +++ b/Strategie/Strategie.cpp Thu May 25 11:30:31 2017 +0000 @@ -18,6 +18,7 @@ signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN signed short x_robot,y_robot,theta_robot;//La position du robot +signed short target_x_robot, target_y_robot, target_theta_robot;//La position cible du robot utilisé pour continuer une ligne droite signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise @@ -331,6 +332,9 @@ #endif localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); GoStraight(localData2, 0, 0, localData5); + //target_x_robot = x_robot + cos((double)theta_robot)*localData1 + //target_y_robot = + //target_theta_robot = theta_robot; break; case MV_TURN: //Rotation sur place @@ -417,7 +421,7 @@ waitingAckID = ASSERVISSEMENT_RECALAGE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; - localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); + //localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); GoStraight(telemetreDistance, 0, 0, 0); // on reset la distance du telemetre à 0 telemetreDistance = 5000; @@ -501,6 +505,8 @@ if (telemetreDistance == 0){ waitingAckID = SERVO_AX12_ACTION;// instruction.arg1; waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE; + + /////////////////////////////////////////////////////////jhjfhfgjfgh }else if(telemetreDistance == 5000){ // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre waitingAckID = ASSERVISSEMENT_RECALAGE; @@ -594,6 +600,7 @@ gameEtat = ETAT_WARNING_SWITCH_STRATEGIE; } break; + case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart //actual_instruction = instruction.nextLineError; @@ -601,14 +608,49 @@ } break; case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours -#ifdef ROBOT_BIG +/*#ifdef ROBOT_BIG actual_instruction = instruction.nextLineError;// 2 //Modification directe... c'est pas bien mais ça marchait pour le match 5 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; #else actual_instruction = instruction.nextLineError; gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; -#endif - gameEtat = ETAT_END; +#endif */ + //gameEtat = ETAT_END; + switch(instruction.order) + { + case MV_COURBURE: + //TODO + gameEtat = ETAT_END; + break; + case MV_LINE: + //Il faut modifier l'instruction pour la transformer en xyT + localData1 = instruction.arg1;//On sauvegarde la distance à parcourir + instruction.arg1 = target_x_robot; + //x_robot + cos((double)theta_robot)*localData1;//On crée la nouvelle position en X + instruction.arg2 = target_y_robot; + //y_robot + sin((double)theta_robot)*localData1;//On crée la nouvelle position en y + instruction.arg3 = theta_robot; + instruction.order = MV_XYT; + //instruction.arg1 = 300;//On crée la nouvelle position en X + //instruction.arg2 = ;//On crée la nouvelle position en y + //instruction.arg3 = theta_robot; + //instruction.order = MV_XYT; + gameEtat = ETAT_GAME_PROCESS_INSTRUCTION; + break; + case MV_TURN: + //TODO + gameEtat = ETAT_END; + break; + case MV_XYT: + //TODO envoyer l'instruction de nouveau + gameEtat = ETAT_END; + break; + + default: + gameEtat = ETAT_END; + break; + } + break; case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur actual_instruction = instruction.nextLineError;