Diff: Strategie/Strategie.cpp

Revision:

37:fca332b64b42

Parent:

36:6dd30780bd8e

Child:

38:76f886a1c8e6

diff -r 6dd30780bd8e -r fca332b64b42 Strategie/Strategie.cpp
--- a/Strategie/Strategie.cpp	Wed May 02 20:40:57 2018 +0000
+++ b/Strategie/Strategie.cpp	Thu May 03 22:35:27 2018 +0000
@@ -85,6 +85,9 @@
 unsigned char countAliveCard = 0; 
 unsigned char ligne=0;
 
+float angle_moyen_balise_IR = 0.0;
+
+
 signed char Strategie = 0; //N° de la strategie (1-10)
 
 unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo
@@ -1112,6 +1115,8 @@
             loadAllInstruction(Strategie);//Mise en cache de toute les instructions
             led3=1;
             
+            SendRawId(GLOBAL_START);
+            
             gameEtat = ETAT_GAME_WAIT_FOR_JACK;
             if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE) 
             {
@@ -1586,7 +1591,7 @@
         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_WARNING_SWITCH_STRATEGIE;
+                gameEtat = ETAT_EVITEMENT;
             }
         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
@@ -1670,6 +1675,48 @@
             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
             ingnorBaliseOnce = 1;
         break;
+        
+        case ETAT_EVITEMENT :
+            E_Evitement EvitEtat= ETAT_INIT_EVITEMENT;
+            while(1)
+            {
+                canProcessRx();
+                
+                switch(EvitEtat)
+                {
+                    case ETAT_INIT_EVITEMENT:
+                        EvitEtat = ETAT_ESTIMATION_POSITION;
+                        break;
+                        
+                    case ETAT_ESTIMATION_POSITION :
+                        wait_ms(100);
+                        Rotate(3600);
+                        EvitEtat = ETAT_ESTIMATION_POSITION_ROTATION_ACK;
+                        waitingAckID = ASSERVISSEMENT_ROTATION;
+                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                        break;
+                        
+                    case ETAT_ESTIMATION_POSITION_ROTATION_ACK:
+                        if(waitingAckID == 0 && waitingAckFrom == 0)
+                            EvitEtat = ETAT_ESTIMATION_POSITION_ROTATION_ACK_END;
+                        break;
+                        
+                    case ETAT_ESTIMATION_POSITION_ROTATION_ACK_END:
+                        if(angle_moyen_balise_IR >= 13.5 && angle_moyen_balise_IR <= 14.5)
+                        {
+                            wait_ms(100);
+                            SendRawId(ASSERVISSEMENT_STOP);
+                            EvitEtat = ETAT_FIN_EVITEMENT;
+                        }
+                        break;
+                        
+                    case ETAT_FIN_EVITEMENT:
+                        break;
+                }
+            }
+            
+            
+            break;
             
             
             
@@ -1829,6 +1876,20 @@
             
             case BALISE_STOP:
                 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
+                
+                signed char fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
+                signed char debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4; 
+                
+                if(debut_angle_detection > fin_angle_detection)
+                {
+                    angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0;
+                    if(angle_moyen_balise_IR > 15.0)
+                        angle_moyen_balise_IR-=15.0;
+                }
+                else
+                    angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
+                
+                
                 if (instruction.order != MV_TURN && instruction.order != ACTION ){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot, 
                     if(needToStop() != 0 && ingnorBaliseOnce ==0) {
                         if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT)