Diff: Strategie/Strategie.cpp

Revision:

18:cc5fec34ed9c

Parent:

16:7321fb3bb396

Child:

20:de595e4ff01d

diff -r d1594579eec6 -r cc5fec34ed9c Strategie/Strategie.cpp
--- a/Strategie/Strategie.cpp	Fri May 19 17:14:07 2017 +0000
+++ b/Strategie/Strategie.cpp	Mon May 22 15:01:49 2017 +0000
@@ -14,7 +14,7 @@
 unsigned short waitingAckID = 0;//L'id du ack attendu
 unsigned short waitingAckFrom = 0;//La provenance du ack attendu
 char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre 
-
+//unsigned short telemetreDistance = 0;
 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
@@ -36,7 +36,7 @@
 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
 
-InterruptIn jack(p24); //  entrée analogique en interruption pour le jack
+InterruptIn jack(p25); //  entrée analogique en interruption pour le jack
 #endif
 unsigned char checkCurrent = 0;
 unsigned char countAliveCard = 0; 
@@ -242,9 +242,10 @@
                 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
             }
             SetOdometrie(ODOMETRIE_BIG_POSITION, POSITION_DEBUT_X,localData3,localData2);
-#endif /*
+#endif 
+#ifdef ROBOT_SMALL
             SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,POSITION_DEBUT_Y,POSITION_DEBUT_T);
-#endif*/
+#endif
         break;
         case ETAT_GAME_WAIT_FOR_JACK:
             //On attend le jack
@@ -303,6 +304,7 @@
                     {
                         localData1 = -localData1;//Inversion de la direction
                     }
+                    
                     BendRadius(instruction.arg1, instruction.arg3, localData1, localData5);
                 break;
                 case MV_LINE://Ligne droite
@@ -390,20 +392,36 @@
                     GoStraight(localData2, localData5, localData3, 0);
                 break;
                 case ACTION:
-                    
-                    waitingAckID = SERVO_AX12_ACTION;
+                    int tempo = 0;
+                    waitingAckID= SERVO_AX12_ACTION;
                     waitingAckFrom = ACKNOWLEDGE_AX12;
-                    if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
+                    tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
+                    if(tempo == 1){
                         //L'action est spécifique
                         if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
-                            wait_us(200);
+                            
                             actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
                             gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                         } else {
                             gameEtat = ETAT_GAME_WAIT_ACK;
                         }
+                        #ifdef ROBOT_SMALL
+                            actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+                            gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+                        #endif
                         return;
-                    } else {
+                     #ifdef ROBOT_SMALL   
+                    } else if (tempo == 2) {
+                        // on est dans le cas de l'avance selon le telemetre
+                        waitingAckID = ASSERVISSEMENT_RECALAGE;
+                        waitingAckFrom = ACKNOWLEDGE_MOTEUR;
+                        
+                        localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
+                        GoStraight(telemetreDistance, 0, 0, 0);
+                        // on reset la distance du telemetre à 0
+                        telemetreDistance = 5000;
+                        #endif
+                    }else{
                         //C'est un AX12 qu'il faut bouger
                         //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
                         //AX12_enchainement++;
@@ -479,8 +497,15 @@
                         case ACTION:
                             
                             if (modeTelemetre == 0){
-                                waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;  
-                                waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
+                                if (telemetreDistance == 0){
+                                    waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;  
+                                    waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
+                                }else if(telemetreDistance == 5000){
+                                    // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
+                                    waitingAckID = ASSERVISSEMENT_RECALAGE;
+                                    waitingAckFrom = INSTRUCTION_END_MOTEUR;
+                                    telemetreDistance = 0;
+                                }
                             }else{ // si on attend la reponse du telemetre  
                                 //modeTelemetre = 1; 
                                 waitingAckID = OBJET_SUR_TABLE;
@@ -601,6 +626,7 @@
         break;
         case ETAT_END_LOOP:
             //Rien, on tourne en rond
+            
         break;
         default:
         
@@ -651,6 +677,7 @@
             case INSTRUCTION_END_MOTEUR:
             case INSTRUCTION_END_IHM:
             case INSTRUCTION_END_AX12:
+                
                 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
                     waitingAckFrom = 0;
                     waitingAckID = 0;
@@ -742,7 +769,7 @@
             
             case BALISE_STOP:
                 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
-                if (instruction[actual_instruction].order == MV_TURN){ //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot, 
+                //if (instruction[actual_instruction].order != MV_TURN){ //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)
                         {
@@ -755,7 +782,7 @@
                             }
                         }
                     }
-                }
+                //}
                 ingnorBaliseOnce = 0;
             break;