homologation gros robot et test avec les ack de la carte a tout faire
Fork of CRAC-Strat_2017_HOMOLOGATION_PETIT_ROBOT by
Diff: Strategie/Strategie.cpp
- Revision:
- 15:c2fc239e85df
- Parent:
- 14:c8fc06c4887f
- Child:
- 16:7321fb3bb396
diff -r c8fc06c4887f -r c2fc239e85df Strategie/Strategie.cpp --- 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; }