Diff: Strategie/Strategie.cpp

Revision:

15:c2fc239e85df

Parent:

14:c8fc06c4887f

Child:

16:7321fb3bb396

--- a/Strategie/Strategie.cpp	Fri Mar 31 16:20:26 2017 +0000
+++ b/Strategie/Strategie.cpp	Thu May 11 12:55:52 2017 +0000
@@ -25,13 +25,17 @@
 //unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS,CHECK_AX12,CHECK_POMPES};
 //unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES};
 
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_MOTEUR};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_MOTEUR};
+
+InterruptIn jack(p25); //  entrée analogique en interruption pour le jack
+#else
+//unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
+//unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
+
 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
 
-InterruptIn jack(p25); //  entrée analogique en interruption pour le jack
-#else
-unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
-unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
 InterruptIn jack(p24); //  entrée analogique en interruption pour le jack
 #endif
 unsigned char checkCurrent = 0;
@@ -315,6 +319,13 @@
                             localData5 = 0;
                         }
                     }
+                    
+                    if(InversStrat == 1) {
+                            /*if (instruction.direction == FORWARD) instruction.direction = BACKWARD;
+                            else instruction.direction = FORWARD;*/
+                            instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD);
+                        }
+                        
                     localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
                     GoStraight(localData2, 0, 0, localData5);
                     
@@ -379,23 +390,23 @@
                     GoStraight(localData2, localData5, localData3, 0);
                 break;
                 case ACTION:
-                    SendRawId(0x300);
-                    waitingAckFrom = 0;
-                    waitingAckID = 0;
+                    
+                    waitingAckID = SERVO_AX12_ACTION;
+                    waitingAckFrom = ACKNOWLEDGE_AX12;
                     if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
                         //L'action est spécifique
                         if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
-                            ///wait_us(200);
+                            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_END_INSTRUCTION;
+                            gameEtat = ETAT_GAME_WAIT_ACK;
                         }
                         return;
                     } else {
                         //C'est un AX12 qu'il faut bouger
-                        AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
-                        AX12_enchainement++;
+                        //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
+                        //AX12_enchainement++;
                                                 
                     }
                 break;
@@ -412,12 +423,17 @@
                 cartesCheker.reset();//On reset le timeOut
                 cartesCheker.start();
                 if(AX12_enchainement > 0) {
-                    AX12_processChange();//Il faut lancer le déplacement des AX12
-                    AX12_enchainement = 0;
+                    //AX12_processChange();//Il faut lancer le déplacement des AX12
+                    //AX12_enchainement = 0;
                 }
             } else {//C'est un enchainement
-                actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
-                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
+                if(instruction.order == MV_LINE){
+                      gameEtat =  ETAT_GAME_WAIT_ACK;
+                    
+                }else{
+                    actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+                    gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
+                }
             }
             
         break;
@@ -461,16 +477,15 @@
                             waitingAckFrom = INSTRUCTION_END_MOTEUR;
                         break;
                         case ACTION:
-                            SendRawId(0x302);
-                            waitingAckFrom = 0; //SERVO_AX12_DONE;
-                            waitingAckID = 0;// instruction.arg1;
-                            /*if (modeTelemetre == 0){
-                                
-                            }else{ // si on attend la reponse du telemetre
-                                waitingAckFrom = 0;//TELEMETRE_RECHERCHE_OBJET;
-                                waitingAckID = OBJET_SUR_TABLE;   
-                                //modeTelemetre = 0;  
-                            }*/
+                            
+                            if (modeTelemetre == 0){
+                                waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;  
+                                waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
+                            }else{ // si on attend la reponse du telemetre  
+                                //modeTelemetre = 1; 
+                                waitingAckID = OBJET_SUR_TABLE;
+                                waitingAckFrom = 0; 
+                            }
                         break;
                         default:
                         break;
@@ -497,8 +512,47 @@
                 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
             }
         break;
+        
+        case ETAT_GAME_JUMP_CONFIG:
+            signed int depasX = 1, depasY = 1;  // servent à indiquer le sens de dépassement des coordonnées
+                                                //  1 si l'instruction est plus grande que la position du robot
+                                                // -1 si l'instruction est plus petite que la position du robot
+                                                //  0 si l'instruction et position du robot sont proche de moins de 1cm
+            if (abs(x_robot-instruction.JumpTimeOrX)<10){
+                    depasX = 0;
+            }else if(x_robot > instruction.JumpTimeOrX){
+                    depasX = -1;
+            }
+            
+            if(abs(y_robot-instruction.JumpY)<10){
+                    depasY = 0;
+            }else if(y_robot > instruction.JumpY){
+                    depasY = -1;
+            }
+                
+            gameEtat = ETAT_GAME_JUMP_POSITION;
+        break;
         case ETAT_GAME_JUMP_POSITION:
-            
+            bool Xok = false, Yok = false;
+             
+                if (depasX == 0){
+                    Xok = true;    
+                }else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5){
+                    Xok = true;
+                }
+                
+                if (depasY == 0){
+                    Yok = true;    
+                }else if ((instruction.JumpY - y_robot)*depasY < -5){
+                    Yok = true;
+                }
+                
+                // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
+                if (Xok && Yok){
+                        actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+                        gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
+                    }
+                
         break;
         case ETAT_GAME_WAIT_END_INSTRUCTION:
             if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
@@ -522,7 +576,7 @@
         break;
         case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
 #ifdef ROBOT_BIG
-            actual_instruction = 2;//Modification directe... c'est pas bien mais ça marchait pour le match 5
+            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;
@@ -586,18 +640,21 @@
                 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
                     waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
                 }
-            break;
+            break; 
             
             case ACKNOWLEDGE_BALISE:
             case ACKNOWLEDGE_MOTEUR:
             case ACKNOWLEDGE_IHM:
             case ACKNOWLEDGE_TELEMETRE:
+            case ACKNOWLEDGE_AX12:
             case INSTRUCTION_END_BALISE:
             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;
+                    SendRawId(0x123);
                 }
             break;
 #ifdef ROBOT_BIG
@@ -628,11 +685,11 @@
             break;
             
             case SERVO_AX12_DONE:
-                SendRawId(0x666);
+                SendRawId(POMPE_PWM);
                 /*//SendAck(0x114, SERVO_AX12_DONE);
                 AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
                 
-                /*gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+                gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
                 waitingAckFrom = 0;
                 waitingAckID = 0;*/
                 
@@ -654,10 +711,6 @@
                     
                 }
             break;
-                
-            case TELEMETRE_RECHERCHE_COIN:
-                SendRawId(RESET_TELEMETRE);
-            break;
             
             case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser
                 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
@@ -724,10 +777,19 @@
             break;
             
             case OBJET_SUR_TABLE:
-                if ((gameEtat == ETAT_GAME_WAIT_ACK)&&(modeTelemetre)){
-                    strat_instructions[actual_instruction+1].arg1 = msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
-                    strat_instructions[actual_instruction+1].arg2 = msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
-                }
+                if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
+                        
+                        gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
+                    }
+                else{
+                        
+                        waitingAckFrom = 0;
+                        waitingAckID = 0; 
+                        
+                        strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2);
+                        strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2);
+                    }
+                modeTelemetre = 0;
             break;
         }