Programme carte strategie (disco)
Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac
Diff: Strategie/Strategie.cpp
- Revision:
- 30:a1e37af4bbde
- Parent:
- 29:41e02746041d
- Child:
- 31:833fc481b002
diff -r 41e02746041d -r a1e37af4bbde Strategie/Strategie.cpp --- a/Strategie/Strategie.cpp Fri Apr 06 13:47:19 2018 +0000 +++ b/Strategie/Strategie.cpp Fri Apr 20 09:16:13 2018 +0000 @@ -92,6 +92,7 @@ struct S_Instruction instruction; +char couleur1, couleur2, couleur3; float cptf; int cpt,cpt1; @@ -109,13 +110,12 @@ Button CHECK (0, 420, 400, 200, "Valider"); Button MATCH (0, 50, 400, 320, "Match"); Button DEMONSTRATION (0, 400, 400, 320, "Demo"); - Button TEST_MOTEUR(0, 25, 400, 200, "Moteur"); - Button TEST_BRAS(0, 250, 400, 200, "Bras"); + Button TEST_MOTEUR(0, 25, 400, 200, "Demo immeuble"); + Button TEST_BRAS(0, 250, 400, 200, "Demo Bluetooth"); Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch"); Button SUIVANT(0,380,200,100,"Suivant"); //////////////////////////////////////////////////////////// -void affichage_debug(char *bug); void SendRawId (unsigned short id); void SelectionStrat (unsigned char numeroStrat); void Setflag(void); @@ -126,6 +126,10 @@ void affichage_compteur (int nombre); void effacer_segment(long couleur); +unsigned short telemetreDistance=0; +unsigned short telemetreDistance1=0; +unsigned short telemetreDistance2=0; +unsigned short telemetreDistance3=0; #ifdef ROBOT_BIG @@ -139,6 +143,7 @@ unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR}; unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR}; +InterruptIn jack(PA_6); // entrée analogique en interruption pour le jack #endif @@ -251,7 +256,7 @@ flagSendCan = 1; } -void affichage_debug(E_stratGameEtat Var){ +void affichage_debug(int Var){ int i; int conv=(int)Var; SUIVANT.Draw(ROUGE, 0); @@ -264,10 +269,12 @@ lcd.SetBackColor(BLEU); lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE); } - while(!ack_bluetooth){ - liaison_bluetooth(); + /*while(!ack_bluetooth){ + //liaison_bluetooth(); } - ack_bluetooth=0; + ack_bluetooth=0;*/ + /*while(SUIVANT.Touched()==0); + while(SUIVANT.Touched());*/ } @@ -284,7 +291,13 @@ case INIT : ts.GetState(&TS_State); canProcessRx(); - + + CANMessage trame_Tx = CANMessage(); + trame_Tx.len = 1; + trame_Tx.format = CANStandard; + trame_Tx.type = CANData; + + lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.Clear (LCD_COLOR_WHITE); @@ -327,6 +340,7 @@ else if (FORCE_LAUNCH.Touched()){ etat = CHOIX; gameEtat = ETAT_CONFIG; + while(FORCE_LAUNCH.Touched()); } break; @@ -371,18 +385,20 @@ canProcessRx(); if(TEST_MOTEUR.Touched()) { - Strat = 0x10; + //Strat = 0x10; while(TEST_MOTEUR.Touched()); TEST_MOTEUR.Draw(0xFFF0F0F0, 0); etat = DEMO_EN_COURS; + ModeDemo=0; } if(TEST_BRAS.Touched()) { - Strat = 0x11; + //Strat = 0x11; while(TEST_BRAS.Touched()); TEST_MOTEUR.Draw(0xFFF0F0F0, 0); etat = DEMO_EN_COURS; + ModeDemo=1; } if(RETOUR.Touched()) @@ -391,18 +407,18 @@ while(RETOUR.Touched()); } if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config - if (Strat< 0x10){ // Si la strat est une strat de match, on desactive le mode demo + /*if (Strat< 0x10){ // Si la strat est une strat de match, on desactive le mode demo ModeDemo = 0; } else { // sinon, on active le mode demo, utile pour la fin de la demo ModeDemo = 1; - } + }*/ Ack_strat = 1; wait_ms(10); - tactile_printf("Strat %d, Asser desactive",Strat); + //tactile_printf("Strat %d, Asser desactive",Strat); } - SelectionStrat(Strategie); + //SelectionStrat(Strategie); } break; @@ -412,11 +428,11 @@ lcd.DisplayStringAt(20, LINE(10), (uint8_t *)"DEMONSTRATION EN COURS", LEFT_MODE); - while (etat == DEMO_EN_COURS) + while(etat == DEMO_EN_COURS) { canProcessRx(); - if (Ack_strat == 1) + /*if (Ack_strat == 1) { if(gameEtat == ETAT_CONFIG) { gameEtat = ETAT_GAME_INIT; @@ -427,7 +443,35 @@ } etat = DEMO; flag_strat = 0; + }*/ + if(ModeDemo==0){ + CANMessage msgTx=CANMessage(); + msgTx.id=0x090; // Monter immeuble + msgTx.len=3; + msgTx.format=CANStandard; + msgTx.type=CANData; + // Rayon sur 2 octets + msgTx.data[0]=4; + msgTx.data[1]=3; + msgTx.data[2]=2; + can2.write(msgTx); + ModeDemo==3; + while(1); } + else if (ModeDemo==1){ + CANMessage msgTx=CANMessage(); + msgTx.id=DATA_TELEMETRE; // Monter immeuble + msgTx.len=1; + msgTx.format=CANStandard; + msgTx.type=CANData; + // Rayon sur 2 octets + msgTx.data[0]=0; + can2.write(msgTx); + wait(1); + etat=DEMO; + } + + } Ack_strat = 0; @@ -452,8 +496,11 @@ Cote = 0x0; InversStrat = Cote; etat = TACTIQUE; + trame_Tx.id=CHOICE_COLOR; + trame_Tx.data[0]=Cote; + can2.write(trame_Tx); while(COTE_BLEU.Touched()); - envoi_bluetooth(1,'9'); + } if(COTE_JAUNE.Touched()) @@ -461,6 +508,9 @@ Cote = 0x1; InversStrat= Cote; etat = TACTIQUE; + trame_Tx.id=CHOICE_COLOR; + trame_Tx.data[0]=Cote; + can2.write(trame_Tx); while(COTE_JAUNE.Touched()); } @@ -620,6 +670,7 @@ switch(gameEtat) { + case ETAT_CHECK_CARTES: /* Il faut faire une boucle pour verifier toutes les cartes les une apres les autres @@ -648,7 +699,7 @@ if(countAliveCard >= NOMBRE_CARTES) { gameEtat = ETAT_CONFIG; flag=1; - tactile_printf("Selection couleur et strategie"); + //tactile_printf("Selection couleur et strategie"); } else { gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement @@ -715,7 +766,7 @@ { etat = AFF_WAIT_JACK; } - tactile_printf("Attente du JACK."); + //tactile_printf("Attente du JACK."); setAsservissementEtat(1);//On réactive l'asservissement jack.mode(PullDown); // désactivation de la résistance interne du jack jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack @@ -756,7 +807,7 @@ jack.fall(NULL);//On désactive l'interruption du jack SendRawId(GLOBAL_START); Jack=0; //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV - tactile_printf("Start");//Pas vraiment utile mais bon + //tactile_printf("Start");//Pas vraiment utile mais bon break; case ETAT_GAME_LOAD_NEXT_INSTRUCTION: /* @@ -788,7 +839,7 @@ actionPrecedente = MV_COURBURE; waitingAckID = ASSERVISSEMENT_COURBURE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; - if(instruction.nextActionType == ENCHAINEMENT) { + if(instruction.nextActionType == ENCHAINEMENT){ MV_enchainement++; localData5 = 1; } else { @@ -945,8 +996,8 @@ break; case ACTION: int tempo = 0; - waitingAckID= SERVO_AX12_ACTION; - waitingAckFrom = ACKNOWLEDGE_AX12; + waitingAckID= ACK_ACTION; //On veut un ack de type action + waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3); if(tempo == 1){ //L'action est spécifique @@ -954,7 +1005,8 @@ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; - } else { + } + else { gameEtat = ETAT_GAME_WAIT_ACK; } #ifdef ROBOT_SMALL @@ -998,9 +1050,9 @@ } } else {//C'est un enchainement if(instruction.order == MV_LINE){ - gameEtat = ETAT_GAME_WAIT_ACK; - - }else{ + 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 } @@ -1011,18 +1063,21 @@ /* Attente de l'ack de l'instruction */ - if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continu + canProcessRx(); + if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue //if(true) { cartesCheker.stop(); if(instruction.nextActionType == JUMP) { if(instruction.jumpAction == JUMP_POSITION) { gameEtat = ETAT_GAME_JUMP_POSITION; - } else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time + } + else {//Pour eviter les erreurs, on dit que c'est par défaut un jump time gameEtat = ETAT_GAME_JUMP_TIME; cartesCheker.reset();//On reset le timeOut cartesCheker.start(); } - } else if(instruction.nextActionType == WAIT) { + } + else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION; switch(instruction.order) { @@ -1050,8 +1105,8 @@ if (modeTelemetre == 0){ if (telemetreDistance == 0){ - waitingAckID = SERVO_AX12_ACTION;// instruction.arg1; - waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE; + waitingAckID = ACK_ACTION;// ack de type action + waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs }else if(telemetreDistance == 5000){ // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre waitingAckID = ASSERVISSEMENT_RECALAGE; @@ -1067,17 +1122,21 @@ default: break; } - } else { + } + else { gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante } - } else if(cartesCheker.read_ms () > 50){ + } + else if(cartesCheker.read_ms () > 50){ cartesCheker.stop(); if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur actual_instruction = instruction.nextLineError; gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; - } else { + } + else { gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction + //gameEtat=ETAT_GAME_WAIT_END_INSTRUCTION; } } break; @@ -1132,6 +1191,7 @@ break; case ETAT_GAME_WAIT_END_INSTRUCTION: + canProcessRx(); if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée 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 @@ -1267,29 +1327,6 @@ if(FIFO_occupation!=0) { int identifiant=msgRxBuffer[FIFO_lecture].id; switch(identifiant) { - case ECRAN_PRINTF_1: - for(useless1 = 0; useless1 <8; useless1++) { - test[useless1] = msgRxBuffer[FIFO_lecture].data[useless1]; - } - break; - case ECRAN_PRINTF_2: - for(useless1 = 0; useless1 <8; useless1++) { - test[useless1+8] = msgRxBuffer[FIFO_lecture].data[useless1]; - } - break; - case ECRAN_PRINTF_3: - for(useless1 = 0; useless1 <8; useless1++) { - test[useless1+2*8] = msgRxBuffer[FIFO_lecture].data[useless1]; - } - break; - case ECRAN_PRINTF_4: - for(useless1 = 0; useless1 <8; useless1++) { - test[useless1+3*8] = msgRxBuffer[FIFO_lecture].data[useless1]; - } - break; - case ECRAN_PRINTF_CLEAR: - etat = CHOIX; - break; case ALIVE_MOTEUR: if (etat == ATT) { @@ -1325,28 +1362,33 @@ } break; - case ALIVE_ACTIONNEURS_AVANT: + case ALIVE_ACTIONNEURS_AVANT: //pas de break donc passe directement dans ECRAN_ALL_CHECK mais conserve l'ident initial case ALIVE_ACTIONNEURS_ARRIERE: - case ALIVE_AX12: + case ALIVE_HERKULEX: case ECRAN_ALL_CHECK: if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) { waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne } break; - case ACKNOWLEDGE_BALISE: + + + + /////////////////////////////////////Acknowledges de Reception de la demande d'action//////////////////////////////////////// + case ACKNOWLEDGE_HERKULEX: + case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans INSTRUCTION_END_AX12 mais conserve l'ident initial case ACKNOWLEDGE_MOTEUR: - case ACKNOWLEDGE_TELEMETRE: - case ACKNOWLEDGE_AX12: + case ACKNOWLEDGE_TELEMETRE: + /////////////////////////////////////////////Acknowledges de la fin d'action///////////////////////////////////////////////// case INSTRUCTION_END_BALISE: case INSTRUCTION_END_MOTEUR: - case INSTRUCTION_END_IHM: - case INSTRUCTION_END_AX12: - + case INSTRUCTION_END_HERKULEX: if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) { + //SendRawId(waitingAckID); waitingAckFrom = 0; - waitingAckID = 0; + waitingAckID = 0; } + break; #ifdef ROBOT_BIG case ODOMETRIE_BIG_POSITION: @@ -1358,29 +1400,35 @@ theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8); break; - case SERVO_AX12_SETGOAL: + //case SERVO_AX12_SETGOAL: //SendAck(0x114, SERVO_AX12_SETGOAL); //if(AX12_isLocal(msgRxBuffer[FIFO_lecture].data[0])) //AX12_setGoal(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[2])<<8), msgRxBuffer[FIFO_lecture].data[3]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[4])<<8)); - break; + //break; - case SERVO_AX12_PROCESS: + /*case SERVO_AX12_PROCESS: SendAck(0x114, SERVO_AX12_PROCESS); //AX12_processChange(1); break; case SERVO_AX12_DONE: SendRawId(POMPE_PWM); - /*//SendAck(0x114, SERVO_AX12_DONE); + //SendAck(0x114, SERVO_AX12_DONE); AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0]))); gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; waitingAckFrom = 0; waitingAckID = 0;*/ - break; - + //break; + case ACK_ACTION: + if(waitingAckID == msgRxBuffer[FIFO_lecture].id) { + waitingAckFrom = 0; + waitingAckID = 0; + } + break; + case BALISE_DANGER : SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); break; @@ -1413,18 +1461,6 @@ } break; - case ECRAN_CHOICE_START_ACTION: - if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config - if(msgRxBuffer[FIFO_lecture].data[0] == 1) { - runRobotTest(); - } else { - initRobotActionneur(); - } - wait_ms(500); - SendRawId(ECRAN_ACK_CHOICE_START_ACTION); - } - break; - case OBJET_SUR_TABLE: if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){ @@ -1441,9 +1477,43 @@ modeTelemetre = 0; break; - + case RECEPTION_DATA: + telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); + telemetreDistance= (float)telemetreDistance*100*35.5+50; + waitingAckFrom = 0; + waitingAckID = 0; + break; + + case RECEPTION_RECALAGE: // BEURK + telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); + telemetreDistance1=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]); + telemetreDistance2=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]); + telemetreDistance3=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]); + /*telemetreDistance= (float)telemetreDistance*3.3*1159.6-687.5+98; + telemetreDistance1= (float)telemetreDistance1*3.3*1159.6-687.5+98; + telemetreDistance2= (float)telemetreDistance2*3.3*1159.6-687.5+98; + telemetreDistance3= (float)telemetreDistance3*3.3*1159.6-687.5+98; + telemetreDistance-=170; + telemetreDistance1-=170; + telemetreDistance2-=170; + telemetreDistance3-=170;*/ + + /*telemetreDistance= (float)telemetreDistance*100*35.5+50; + telemetreDistance1= (float)telemetreDistance1*100*35.5+50; + telemetreDistance2= (float)telemetreDistance2*100*35.5+50; + telemetreDistance3= (float)telemetreDistance3*100*35.5+50;*/ + + + waitingAckFrom = 0; + waitingAckID = 0; + break; - + case RECEPTION_COULEUR: + couleur1=msgRxBuffer[FIFO_lecture].data[0]; + couleur2=msgRxBuffer[FIFO_lecture].data[1]; + couleur3=msgRxBuffer[FIFO_lecture].data[2]; + break; + } FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; } @@ -1490,87 +1560,87 @@ { canProcessRx(); CANMessage msgTx=CANMessage(); - msgTx.id=ECRAN_CHOICE_STRAT; + //msgTx.id=ECRAN_CHOICE_STRAT; if (RETOUR.Touched()) return -1; while(RETOUR.Touched()); //////////////////////////////STRATEGIE N°1 if (STRAT_1.Touched()){ Strat = 0; - msgTx.data[0] = 0x1; - can2.write(msgTx); + //msgTx.data[0] = 0x1; + //can2.write(msgTx); while(STRAT_1.Touched()); Ack_strat =1; } /////////////////////////////STRATEGIE N°2 if (STRAT_2.Touched()){ Strat = 1; - msgTx.data[0] = 0x2; - can2.write(msgTx); + //msgTx.data[0] = 0x2; + //can2.write(msgTx); while(STRAT_2.Touched()); Ack_strat =1; } //////////////////////////////STRATEGIE N°3 if (STRAT_3.Touched()){ Strat = 2; - msgTx.data[0] = 0x3; - can2.write(msgTx); + //msgTx.data[0] = 0x3; + //can2.write(msgTx); while(STRAT_3.Touched()); Ack_strat =1; } /////////////////////////////STRATEGIE N°4 if (STRAT_4.Touched()){ Strat = 3; - msgTx.data[0] = 0x4; - can2.write(msgTx); + //msgTx.data[0] = 0x4; + //can2.write(msgTx); while(STRAT_4.Touched()); Ack_strat =1; } ///////////////////////////////STRATEGIE N°5 if (STRAT_5.Touched()){ Strat = 4; - msgTx.data[0] = 0x5; - can2.write(msgTx); + //msgTx.data[0] = 0x5; + //can2.write(msgTx); while(STRAT_5.Touched()); Ack_strat =1; } ////////////////////////////////STRATEGIE N°6 if (STRAT_6.Touched()){ Strat = 5; - msgTx.data[0] = 0x6; - can2.write(msgTx); + //msgTx.data[0] = 0x6; + //can2.write(msgTx); while(STRAT_6.Touched()); Ack_strat =1; } /////////////////////////////////STRATEGIE N°7 if (STRAT_7.Touched()){ Strat = 6; - msgTx.data[0] = 0x7; - can2.write(msgTx); + //msgTx.data[0] = 0x7; + //can2.write(msgTx); while(STRAT_7.Touched()); Ack_strat =1; } /////////////////////////////////STRATEGIE N°8 if (STRAT_8.Touched()){ Strat = 7; - msgTx.data[0] = 0x8; - can2.write(msgTx); + //msgTx.data[0] = 0x8; + //can2.write(msgTx); while(STRAT_8.Touched()); Ack_strat =1; } /////////////////////////////////STRATEGIE N°9 if (STRAT_9.Touched()){ Strat = 8; - msgTx.data[0] = 0x9; - can2.write(msgTx); + //msgTx.data[0] = 0x9; + //can2.write(msgTx); while(STRAT_9.Touched()); Ack_strat =1; } ///////////////////////////////////STRATEGIE N°10 if (STRAT_10.Touched()){ Strat = 9; - msgTx.data[0] = 0xA; - can2.write(msgTx); + //msgTx.data[0] = 0xA; + //can2.write(msgTx); while(STRAT_10.Touched()); Ack_strat =1; }