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

--- 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];

--- 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

--- 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;