Alexandre Salconi-Denis
/
ProjetOctopode
Programme de contrôle de l'octopode 4DOF, Theraphosa-Salconi.
CtrlBridge
- fonction quelquonque pour communiquer avec les module
- fonction quelquonque pour faire des recherche dans les module dispo
- autre fonction pour jouer avec MemRegistre
Version 1.2.0
- Ajout d'un mode de simulation pour tester le code avec seulement un contrôleur stm32
directive.cpp
- Committer:
- salco
- Date:
- 2017-01-31
- Revision:
- 41:0b5c14dda54a
- Parent:
- 40:ecef15b1dfad
File content as of revision 41:0b5c14dda54a:
#include "directive.h" Serial pc(SERIAL_TX, SERIAL_RX); int tabIR[12][2]= { {80, 400},{50, 600},{40, 700},{30, 900},{25,1100}, {20,1300},{15,1600},{10,2400},{ 8,2700},{ 7,3000}, { 6,3200},{ 5,3100} }; Directive::Directive():TTask(0)//on veux que cette tache sois exec toute les fois que TTaskGen sexecute. { /* initialize random seed: */ srand (time(NULL)); myMaze = new Labyrinthe(); myMaze->setDirection(UP); //c=0; ssc32 = new Serial(PB_6, PB_7); ctrDesPattes = new Faculter_motrice(ssc32/*pc*/); m_valueCapteurUltrasonic = 0; m_valueCapteurIR = 0; m_valueCapteurProximiter = 0; for(int i =0; i < DIRECTIVE_TABLEAUDECOMMANDE_SIZE; i++)tableauDeCommange[i]= 0; size_tableauDeCommange=0; debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r set explorer \n\r"); myMaze->setMyPos(Labyrinthe::explorer); //myMaze->setC(EXPLORER,C5); //m_CtrlBridge = m_CtrlBridge->getInstance(); //a enlever de commentaire//m_ListDesModules = m_CtrlBridge->findModule(0,1,0,0); //Serial pc(USBTX, USBRX); //ssc32->set_flow_control(0); //debug("\n\r directive Init"); m_ListDesModules = m_CtrlBridge->findModule(0,CAPTEUR,DISTANCE,0); m_ListDesModules.append(m_CtrlBridge->findModule(0,CAPTEUR,PROXIMITEE,0)); for(int i=0; i < m_ListDesModules.size(); i++) debug(DEBUG_DIRECTIVE_TEST,"\n\r m_ListDesModules[%i]:%i",i,m_ListDesModules[i]); m_capteurUltrasonic= m_CtrlBridge->findModule(0,CAPTEUR,ULTRASONIQUE,0x27/*0b100111*/); if(!(m_capteurUltrasonic.empty())) { debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurUltrasonic found"); for(int i=0; i < m_capteurUltrasonic.size(); i++) debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurUltrasonic[%i]:%i",i,m_capteurUltrasonic[i]); } else debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurUltrasonic not found"); m_capteurIR= m_CtrlBridge->findModule(0,CAPTEUR,DISTANCE,0x27/*0b100111*/); if(!(m_capteurIR.empty())) { debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurIR found"); for(int i=0; i < m_capteurIR.size(); i++) debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurIR[%i]:%i",i,m_capteurIR[i]); } else debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurIR not found"); m_capteurProximiter= m_CtrlBridge->findModule(0,CAPTEUR,PROXIMITEE,0x0E/*0b001110*/); if(!(m_capteurProximiter.empty())) { debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurProximiter found"); for(int i=0; i < m_capteurProximiter.size(); i++) debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurProximiter[%i]:%i",i,m_capteurProximiter[i]); } else debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurProximiter not found"); ctrDesPattes->moveUp(); bool getUp=true; while((getUp)&& (! ctrDesPattes->isSeqComplet())) { ctrDesPattes->exec(); if(ctrDesPattes->get_idFrame() >= 5) getUp = false; } followThePath = false; ctrDesPattes->calibre(); #ifdef DEBUG_DIRECTIVE_GRAPHICAL_INTERFACE //analiseMaze(); ///wait(2); //debug("\x1B[2J"); //clear screen /*debug("\x1B[25l");//hide cursor debug("\x1B[;H");*/ //cursor default position //createSLbox(0,0,5,15,"Mode"); /*createDLbox(0,0,10,20,"Menu");//(1,12,10,20,"test2"); setText(1,3,"1) BrainControle"); setText(1,4,"2) Show Maze");*/ //createDLbox(16,0,5,15,"Stage"); myMaze->setC(Labyrinthe::mur,0,0,Labyrinthe::C1); myMaze->setC(Labyrinthe::vide,0,0,Labyrinthe::C2); myMaze->setC(Labyrinthe::mur,0,0,Labyrinthe::C3); myMaze->setC(Labyrinthe::mur,0,0,Labyrinthe::C4); myMaze->setC(Labyrinthe::vide,0,0,Labyrinthe::C5); myMaze->setC(Labyrinthe::vide,0,0,Labyrinthe::C6); myMaze->setC(Labyrinthe::mur,0,0,Labyrinthe::C7); myMaze->setC(Labyrinthe::mur,0,0,Labyrinthe::C8); myMaze->setC(Labyrinthe::mur,0,0,Labyrinthe::C9); myMaze->setC(Labyrinthe::vide,3,0,Labyrinthe::C1); myMaze->setC(Labyrinthe::vide,3,0,Labyrinthe::C2); myMaze->setC(Labyrinthe::mur ,3,0,Labyrinthe::C3); myMaze->setC(Labyrinthe::vide,3,0,Labyrinthe::C4); myMaze->setC(Labyrinthe::vide,3,0,Labyrinthe::C5); myMaze->setC(Labyrinthe::vide,3,0,Labyrinthe::C6); myMaze->setC(Labyrinthe::mur ,3,0,Labyrinthe::C7); myMaze->setC(Labyrinthe::mur ,3,0,Labyrinthe::C8); myMaze->setC(Labyrinthe::mur ,3,0,Labyrinthe::C9); myMaze->setC(Labyrinthe::vide,6,0,Labyrinthe::C1); myMaze->setC(Labyrinthe::mur ,6,0,Labyrinthe::C2); myMaze->setC(Labyrinthe::mur ,6,0,Labyrinthe::C3); myMaze->setC(Labyrinthe::vide,6,0,Labyrinthe::C4); myMaze->setC(Labyrinthe::vide,6,0,Labyrinthe::C5); myMaze->setC(Labyrinthe::mur ,6,0,Labyrinthe::C6); myMaze->setC(Labyrinthe::mur ,6,0,Labyrinthe::C7); myMaze->setC(Labyrinthe::mur ,6,0,Labyrinthe::C8); myMaze->setC(Labyrinthe::mur ,6,0,Labyrinthe::C9); myMaze->setC(Labyrinthe::mur ,0,-3,Labyrinthe::C1); myMaze->setC(Labyrinthe::mur ,0,-3,Labyrinthe::C2); myMaze->setC(Labyrinthe::mur ,0,-3,Labyrinthe::C3); myMaze->setC(Labyrinthe::mur ,0,-3,Labyrinthe::C4); myMaze->setC(Labyrinthe::vide,0,-3,Labyrinthe::C5); myMaze->setC(Labyrinthe::mur ,0,-3,Labyrinthe::C6); myMaze->setC(Labyrinthe::mur ,0,-3,Labyrinthe::C7); myMaze->setC(Labyrinthe::vide,0,-3,Labyrinthe::C8); myMaze->setC(Labyrinthe::vide,0,-3,Labyrinthe::C9); myMaze->setC(Labyrinthe::mur ,3,-3,Labyrinthe::C1); myMaze->setC(Labyrinthe::mur ,3,-3,Labyrinthe::C2); myMaze->setC(Labyrinthe::mur ,3,-3,Labyrinthe::C3); myMaze->setC(Labyrinthe::vide,3,-3,Labyrinthe::C4); myMaze->setC(Labyrinthe::vide,3,-3,Labyrinthe::C5); myMaze->setC(Labyrinthe::vide,3,-3,Labyrinthe::C6); myMaze->setC(Labyrinthe::vide,3,-3,Labyrinthe::C7); myMaze->setC(Labyrinthe::mur ,3,-3,Labyrinthe::C8); myMaze->setC(Labyrinthe::vide,3,-3,Labyrinthe::C9); myMaze->setC(Labyrinthe::mur ,6,-3,Labyrinthe::C1); myMaze->setC(Labyrinthe::mur ,6,-3,Labyrinthe::C2); myMaze->setC(Labyrinthe::mur ,6,-3,Labyrinthe::C3); myMaze->setC(Labyrinthe::vide,6,-3,Labyrinthe::C4); myMaze->setC(Labyrinthe::mur ,6,-3,Labyrinthe::C5); myMaze->setC(Labyrinthe::mur ,6,-3,Labyrinthe::C6); myMaze->setC(Labyrinthe::vide,6,-3,Labyrinthe::C7); myMaze->setC(Labyrinthe::vide,6,-3,Labyrinthe::C8); myMaze->setC(Labyrinthe::mur ,6,-3,Labyrinthe::C9); #endif myMaze->setMyPos(Labyrinthe::explorer); #ifdef DEBUG_DIRECTIVE_TEST_CAPTEUR debug("\x1B[2J"); //clear screen char areaLatt =0; char areaVert =0; int idCommand = 0; while(idCommand != 9) { debug("\n\r press any key"); idCommand = fgetc(pc) - '0'; debug("\x1B[;H"); //cursor default position debug("\x1B[2J"); //clear screen updateModuleValue(); ////////////////////////////// // Traitement du Labyrinthe // ////////////////////////////// debug("\n\r -invert ultrasonicToCm: %02f ",inchToCm(ultrasonicToInch(m_valueCapteurIR))); debug("\n\r -invert IRToCm(%02f): %02f ",ADCTomv(m_valueCapteurUltrasonic),IRToCm(ADCTomv(m_valueCapteurUltrasonic))); //debug("\n\r -IR(value):%i ",m_valueCapteurIR); debug("\n\r -IRToCm(%02f): %02f ",ADCTomv(m_valueCapteurIR),IRToCm(ADCTomv(m_valueCapteurIR))); debug("\n\r -ultrasonic(value):%f ",m_valueCapteurUltrasonic); debug("\n\r -ultrasonicToInch: %02f ",ultrasonicToInch(m_valueCapteurUltrasonic)); debug("\n\r -ultrasonicToCm: %02f ",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic))); debug("\n\r -m_valueCapteurProximiter: %02d ",m_valueCapteurProximiter); //wait(.5); if((inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic))<= 1) /*&& (IRToCm(ADCTomv(m_valueCapteurIR)) <= 80)*/)//enlever parce que il est lock avec des limites { //capteur ultrasson embrouiller/imprecis debug("\n\r -Capteur Ultrasson brouiller"); //addTableauDeCommande(6); } else if(inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)) <= DIRECTIVE_ULTRASONIC_LIMIT_SHORT) { //plus proche que 1 case debug("\n\r -Objet proche %g <= %i cm",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)),DIRECTIVE_ULTRASONIC_LIMIT_SHORT); areaLatt =0; if(IRToCm(ADCTomv(m_valueCapteurIR)) <= DIRECTIVE_IR_LIMIT_SHORT) { /* /---\ |?M?| //vert 0 |.X.| //latt 0 |...| \---/ */ areaVert = 0; } else { /* /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ */ areaVert = 1; } } else if(inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)) <= DIRECTIVE_ULTRASONIC_LIMIT_MIDDLE) { //plus proche que 2 case debug("\n\r -Objet proche %g <= %i cm",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)),DIRECTIVE_ULTRASONIC_LIMIT_MIDDLE); //modifier vue que il ne detecte pas asser loin /* /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ */ areaLatt =0; areaVert =1; } else if(inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)) <= DIRECTIVE_ULTRASONIC_LIMIT_LONG) { //plus proche que 2 case debug("\n\r -Objet proche %g <= %i cm",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)),DIRECTIVE_ULTRASONIC_LIMIT_LONG); //modifier vue que il ne detecte pas asser loin /* /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ */ areaLatt =0; areaVert =1; } else if(inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)) > DIRECTIVE_ULTRASONIC_LIMIT_LONG) { //plus proche que 2 case debug("\n\r -Objet proche %g >= %i cm",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)),DIRECTIVE_ULTRASONIC_LIMIT_LONG); //modifier vue que il ne detecte pas asser loin /* /?V?\ |?V?| //vert 2 |.X.| //latt 0 |...| // objet possiblement a proximiter \---/ */ areaLatt =0; areaVert =2; } /*if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 60) { debug("\n\r -Confirmation IR"); areaVert =2; } else if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 30) { debug("\n\r -Confirmation IR"); areaVert =1; }*/ if(areaVert >= areaLatt) { switch(areaVert) { case 0: /* /---\ |?M?| //vert 0 |.X.| //latt 0 |...| \---/ */ debug("\n\r +---+ "); debug("\n\r |?M?| //vert 0 "); debug("\n\r |.X.| //latt 0 "); debug("\n\r |...| "); debug("\n\r +---+ "); break; case 1://impossible pour le moment IR trop instable// if(areaLatt == 1) { /* /?M?\ |VVV| //vert 1 |.X.| //latt 1 |...| // objet possiblement a proximiter \---/ */ debug("\n\r +-M-+ "); debug("\n\r |VVV| //vert 1 "); debug("\n\r |.X.| //latt 1 "); debug("\n\r |...| // objet possiblement a proximiter "); debug("\n\r +---+ "); } else { /* /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ */ debug("\n\r +-M-+ "); debug("\n\r |?V?| //vert 1 "); debug("\n\r |.X.| //latt 0 "); debug("\n\r |...| "); debug("\n\r +---+ "); } break; case 2: switch(areaLatt) { case 0://impossible pour le moment IR trop instable// /* /?V?\ |?V?| //vert 2 |.X.| //latt 0 |...| // objet possiblement a proximiter \---/ */ debug("\n\r +?V?+ "); debug("\n\r |?V?| //vert 2 "); debug("\n\r |.X.| //latt 0 "); debug("\n\r |...| // objet possiblement a proximiter "); debug("\n\r +---+ "); break; case 1://impossible pour le moment IR trop instable// /* /?V?\ |VVV| //vert 2 |.X.| //latt 1 |...| // objet possiblement a proximiter \---/ */ debug("\n\r +-M-+ "); debug("\n\r |VVV| //vert 2 "); debug("\n\r |.X.| //latt 1 "); debug("\n\r |...| // objet possiblement a proximiter "); debug("\n\r +---+ "); break; case 2: /* /VVV\ |VVV| //vert 2 |.X.| //latt 2 |...| \---/ */ debug("\n\r +VVV+ "); debug("\n\r |VVV| //vert 2 "); debug("\n\r |.X.| //latt 2 "); debug("\n\r |...| "); debug("\n\r +---+ "); break; } break; } } /*if((ultrasonicToInch(m_valueCapteurUltrasonic)< 1) && (IRToCm(ADCTomv(m_valueCapteurIR)) <= 80)) { //capteur ultrasson embrouiller/imprecis debug(DEBUG_DIRECTIVE_TEST,"\n\r -Capteur Ultrasson brouiller"); //addTableauDeCommande(6); } else if(ultrasonicToInch(m_valueCapteurUltrasonic) >=24) { //plus proche que 10pouce debug(DEBUG_DIRECTIVE_TEST,"\n\r -Objet proche 2pi"); areaLatt =2; areaVert =2; } else if(ultrasonicToInch(m_valueCapteurUltrasonic) >=12) { //plus proche que 10pouce debug(DEBUG_DIRECTIVE_TEST,"\n\r -Objet proche 2pi"); areaLatt =1; areaVert =1; } debug(DEBUG_DIRECTIVE_TEST,": %i",ultrasonicToInch(m_valueCapteurUltrasonic)); if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 60) { debug(DEBUG_DIRECTIVE_TEST,"\n\r -Confirmation IR"); areaVert =2; } else if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 30) { debug(DEBUG_DIRECTIVE_TEST,"\n\r -Confirmation IR"); areaVert =1; } debug(DEBUG_DIRECTIVE_TEST,": %i",IRToCm(ADCTomv(m_valueCapteurIR)));*/ } #endif } Directive::~Directive() { if(ctrDesPattes) delete ctrDesPattes; if(ssc32) delete ssc32; if(myMaze) delete myMaze; } void Directive::task(void)//ALL CODE HERE// { #ifdef DEBUG_DIRECTIVE_MINIMAL_PRESENTATION if(pc.readable()) { int idCommand=0; idCommand = fgetc(pc) - '0'; string mymap; switch(idCommand) { case 1: debug("\n\r 1: Show Map "); mymap = myMaze->showMap(); debug("\n\r Labyrinthe map: X:%02i Y:%02i \n\r",(signed char)(myMaze->getX()),(signed char)(myMaze->getY())); //char caseCaract = 0; debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+6])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+3])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i])); debug("\n\r"); break; case 2: debug("\n\r *2: Show tableauDeCommange[]"); debug("\n\r -TAB:"); for(int i =0; i<DIRECTIVE_TABLEAUDECOMMANDE_SIZE; i++) debug("[%02x]",tableauDeCommange[i]); if(followThePath == true) debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r EN MODE FOLLOW THE PATH"); else debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r HORS MODE FOLLOW THE PATH"); break; case 3: debug("\n\r *3: Show value Capteurs"); debug("\n\r -IRToCm(%02f): %02f",ADCTomv(m_valueCapteurIR),IRToCm(ADCTomv(m_valueCapteurIR))); debug("\n\r -ultrasonicToInch to cm: %02f",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic))); debug("\n\r -m_valueCapteurProximiter: %02d",m_valueCapteurProximiter); break; default: debug("\n\n\r Press key 0-9 to select an option"); debug("\n\r *1: Show Map"); debug("\n\r *2: Show tableauDeCommange[]"); debug("\n\r *3: Show value Capteurs"); break; } } #endif #ifdef DEBUG_BOOT_GRAPHICAL_INTERFACE debug("\x1B[2J"); //clear screen debug("\x1B[25l");//hide cursor debug("\x1B[;H"); //cursor default position debug("\n\r--------In task directive--------"); for(int i = 0; i<size_tableauDeCommange; i++) debug("\n\r tableauDeCommange[%i]: %x",i,tableauDeCommange[i]); /* createDLbox(0,0,10,25,"BrainControle"); setText(1,3,"1) Move Up"); setText(1,4,"2) Move Down"); setText(1,5,"3) Move Left"); setText(1,6,"4) Move Right"); int idCommand; do{ if(pc.readable()){ idCommand = fgetc(pc) - '0'; switch(idCommand) { case 1: myMaze->goUp(); break; case 2: myMaze->goDown(); break; case 3: myMaze->goLeft(); break; case 4: myMaze->goRight(); break; } myMaze->setMyPos(Labyrinthe::explorer); string mymap = myMaze->showMap(); debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r Labyrinthe map: X:%02x Y:%02x \n\r",myMaze->getX(),myMaze->getY()); for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i+6]); debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i+3]); debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i]); debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); #ifdef DBG_MAIN_DELAY_SEC debug("\n\r\n wait \n\r\n"); wait(DBG_MAIN_DELAY_SEC); #endif debug("\x1B[2J"); //clear screen debug("\x1B[;H"); //cursor default position createDLbox(0,0,10,25,"BrainControle"); setText(1,3,"1) Move Up"); setText(1,4,"2) Move Down"); setText(1,5,"3) Move Left"); setText(1,6,"4) Move Right"); } }while(1); */ #endif //debug(DEBUG_DIRECTIVE_TEST,"\n\rIn task directive"); #ifndef DEBUG_DIRECTIVE_LEG_DISABLE if(ctrDesPattes->isSeqComplet()) { //////////////// // Inspection // //////////////// //updateModuleValue(); if((tableauDeCommange[0] == TBL_CMD_MARCHE) && (size_tableauDeCommange == 1)) { //addTableauDeCommande(0x81); //addTableauDeCommande(0x82); //mabey//addTableauDeCommande(0x83); addTableauDeCommande(0x84); //addTableauDeCommande(0x85); //mabey//addTableauDeCommande(0x86); addTableauDeCommande(0x87); addTableauDeCommande(0x88); } if(tableauDeCommange[0] == 0) { //pas sur que c'Est tout ce qui doit etre ici mais je vois pas quoi d'autre pour le moment. //////////////// // Inspection // //////////////// //flag.append(1,0);//0x02); /*for(int i=0; i<m_ListDesModules.length(); ++i) { wait_us(300); flag.clear(); flag.append(1,7); data.clear(); debug("\n\r result: %d",m_CtrlBridge->send(m_ListDesModules.at(i),flag,data)); debug("\n\r flag: %d",flag[0]); debug("\n\r data: %x",data[0]); }*/ updateModuleValue(); ////////////////////////////// // Traitement du Labyrinthe // ////////////////////////////// #ifdef DEBUG_DIRECTIVE_LABYRINTH debug("\n\r -IRToCm(%02f): %02f",ADCTomv(m_valueCapteurIR),IRToCm(ADCTomv(m_valueCapteurIR))); debug("\n\r -ultrasonicToInch: %02f",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic))); debug("\n\r -m_valueCapteurProximiter: %02d",m_valueCapteurProximiter); #endif #endif //DEBUG_DIRECTIVE_LEG_DISABLE if(followThePath == true) debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r EN MODE FOLLOW THE PATH"); else debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r HORS MODE FOLLOW THE PATH"); analiseMaze(); char nextCase=0; /*switch(myMaze->getDirection()) { case UP: nextCase = myMaze->getC(myMaze->getX(),myMaze->getY()+1); break; case DOWN: nextCase = myMaze->getC(myMaze->getX(),myMaze->getY()-1); break; case LEFT: nextCase = myMaze->getC(myMaze->getX()-1,myMaze->getY()); break; case RIGHT: nextCase = myMaze->getC(myMaze->getX()+1,myMaze->getY()); break; }*/ if(( myMaze->getC_Foward() == Labyrinthe::vide ) || (myMaze->getC_Foward() == Labyrinthe::pasExplorer )) followThePath = false; if(followThePath == false) { nextCase = myMaze->getC_Foward(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r nextCase: [%02x]",nextCase); switch(nextCase ) { case Labyrinthe::vide : #ifdef DEBUG_DIRECTIVE_LEG_DISABLE debug("nextCase Vide\n\r"); #endif debug(DEBUG_DIRECTIVE_TEST," Javance"); #ifndef DEBUG_DIRECTIVE_LEG_DISABLE addTableauDeCommande(TBL_CMD_MARCHE); #endif //if(followThePath == true) //followThePath = false; myMaze->moveFoward(); myMaze->setMyPos(Labyrinthe::explorer); break; case Labyrinthe::explorer :// ici pt amiliorer debug(DEBUG_DIRECTIVE_TEST," \n\rnext case deja explorer"); if(rand()%2) { debug(DEBUG_DIRECTIVE_TEST,"\n\r jeverifie si alternative"); /*#ifndef DEBUG_DIRECTIVE_LEG_DISABLE addTableauDeCommande(TBL_CMD_MARCHE); #endif*/ nextCase = checkOtherWay(myMaze->getDirection(),1); turnRightDirection(myMaze->getDirection(),nextCase); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rnextcase: %i",nextCase); switch(nextCase) { // on verifie si on a une direction possible case UP: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne UP"); turnRightDirection(myMaze->getDirection(),UP); myMaze->setDirection(UP); break; case DOWN: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne DOWN"); turnRightDirection(myMaze->getDirection(),DOWN); myMaze->setDirection(DOWN); break; case LEFT: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne LEFT"); turnRightDirection(myMaze->getDirection(),LEFT); myMaze->setDirection(LEFT); break; case RIGHT: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne RIGHT"); turnRightDirection(myMaze->getDirection(),RIGHT); myMaze->setDirection(RIGHT); break; default: //aucun autre chemin debug(DEBUG_DIRECTIVE_TEST,"\n\r aucune autre place alors J'avance"); myMaze->moveFoward(); myMaze->setMyPos(Labyrinthe::explorer); #ifndef DEBUG_DIRECTIVE_LEG_DISABLE addTableauDeCommande(TBL_CMD_MARCHE); #endif break; } } else { debug(DEBUG_DIRECTIVE_TEST,"\n\r jeverifie pas si alternative"); myMaze->moveFoward(); #ifndef DEBUG_DIRECTIVE_LEG_DISABLE addTableauDeCommande(TBL_CMD_MARCHE); #endif } //myMaze->moveFoward(); //myMaze->setMyPos(Labyrinthe::explorer); break; case Labyrinthe::mur: debug(DEBUG_DIRECTIVE_TEST,"\n\r UN MURRRRRRRR\n\r"); if(searchNewWay()) { debug(DEBUG_DIRECTIVE_TEST,"\n\r New Path found "); followThePath = true; } else { debug(DEBUG_DIRECTIVE_TEST,"\n\r New Path not found "); } /* switch(checkOtherWay(myMaze->getDirection()))// on verifie si on a une direction possible { case UP: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne UP "); myMaze->setDirection(UP); //myMaze->moveFoward(); break; case DOWN: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne DOWN "); myMaze->setDirection(DOWN); //myMaze->moveFoward(); break; case LEFT: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne LEFT"); myMaze->setDirection(LEFT); //myMaze->moveFoward(); break; case RIGHT: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne RIGHT"); myMaze->setDirection(RIGHT); //myMaze->moveFoward(); break; default://aucun autre chemin /debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne completement "); if(myMaze->getDirection() %2) myMaze->setDirection(myMaze->getDirection()+1); else myMaze->setDirection(myMaze->getDirection()-1); //myMaze->moveFoward(); break; } myMaze->setMyPos(Labyrinthe::explorer);*/ break; case Labyrinthe::pasExplorer ://pas sur debug(DEBUG_DIRECTIVE_TEST," pasExplorer"); #ifndef DEBUG_DIRECTIVE_LEG_DISABLE addTableauDeCommande(TBL_CMD_MARCHE); #endif break; } } else { // on suis un chemin debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rmode follow the path\n\n\r"); positionXY tempPosition; //std::list<positionXY>::iterator it=bufferNewWay.begin(); for (std::list<positionXY>::iterator it=bufferNewWay.begin(); it != bufferNewWay.end(); ++it) { //tempPosition = *(it); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posX: %i",(*it).posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posY: %i",(*it).posY); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.direction: %i",(*it).direction); } //std::cout << ' ' << *it; if((bufferNewWay.front()).direction != myMaze->getDirection()) //turn time { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r le chemin a pas la meme direction"); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r current dir : %i",myMaze->getDirection()); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r next dir : %i",(bufferNewWay.front()).direction); addTableauDeCommande(TBL_CMD_RECULE); addTableauDeCommande(TBL_CMD_RECULE); turnRightDirection(myMaze->getDirection(),(bufferNewWay.front()).direction); switch((bufferNewWay.front()).direction) { case UP: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne UP"); myMaze->setDirection(UP); /*if(((bufferNewWay.front()).direction + myMaze->getDirection()) == (UP + DOWN)) { if(rand()%2) { addTableauDeCommande(TBL_CMD_TURN_LEFT); addTableauDeCommande(TBL_CMD_TURN_LEFT); } else { addTableauDeCommande(TBL_CMD_TURN_RIGHT); addTableauDeCommande(TBL_CMD_TURN_RIGHT); } }*/ break; case DOWN: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne DOWN"); myMaze->setDirection(DOWN); break; case LEFT: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne LEFT"); myMaze->setDirection(LEFT); break; case RIGHT: debug(DEBUG_DIRECTIVE_TEST,"\n\r Je tourne RIGHT"); myMaze->setDirection(RIGHT); break; } //bufferNewWay.pop_front(); } else { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r le chemin a la meme direction"); //myMaze->getXY_Foward(tempPosition.posX,tempPosition.posY); tempPosition.posX = myMaze->getX(); tempPosition.posY = myMaze->getY(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r current X: %i",tempPosition.posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r current Y: %i",tempPosition.posY); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r buffer X: %i",(bufferNewWay.front()).posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r buffer Y: %i",(bufferNewWay.front()).posY); if((tempPosition.posX == (bufferNewWay.front()).posX) && (tempPosition.posY == (bufferNewWay.front()).posY)) { updateMaze(); nextCase = myMaze->getC_Foward(); if(nextCase == Labyrinthe::mur) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r problem"); followThePath = false; } else { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r aucun problem"); #ifndef DEBUG_DIRECTIVE_LEG_DISABLE addTableauDeCommande(TBL_CMD_MARCHE); #endif myMaze->moveFoward(); myMaze->setMyPos(Labyrinthe::explorer); bufferNewWay.pop_front(); } } else debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r problem"); } if(bufferNewWay.empty()) followThePath = false; /*do{ //tempPosition.posX = (bufferNewWay.front()).posX; //tempPosition.posY = (bufferNewWay.front()).posY; tempPosition = bufferNewWay.front(); bufferNewWay.pop_front(); debug("\n\r bufferNewWay: %i",bufferNewWay.size()); debug("\n\rtempPosition.posX: %i",tempPosition.posX); debug("\n\rtempPosition.posY: %i",tempPosition.posY); }while(bufferNewWay.size() != 0);*/ } #ifdef DEBUG_DIRECTIVE_SHOW_MAP_LABYRINTH string mymap = myMaze->showMap(); debug("\n\r Labyrinthe map: X:%02i Y:%02i \n\r",(signed char)(myMaze->getX()),(signed char)(myMaze->getY())); //char caseCaract = 0; debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+6])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+3])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i])); debug("\n\r"); #endif #ifndef DEBUG_DIRECTIVE_LEG_DISABLE /*if((ultrasonicToInch(m_valueCapteurUltrasonic)< 1) && (IRToCm(ADCTomv(m_valueCapteurIR)) <= 80)) //capteur ultrasson embrouiller/imprecis { debug(DEBUG_DIRECTIVE_TEST,"\n\r -Capteur Ultrasson brouiller"); addTableauDeCommande(6); } else if(ultrasonicToInch(m_valueCapteurUltrasonic) < 10) { //plus proche que 10pouce debug(DEBUG_DIRECTIVE_TEST,"\n\r -Objet proche"); if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 10) { debug(DEBUG_DIRECTIVE_TEST,"\n\r -Confirmation IR"); addTableauDeCommande(7); //ici potentielle scaning } else { addTableauDeCommande(4); addTableauDeCommande(4); addTableauDeCommande(4); } } else { debug(DEBUG_DIRECTIVE_TEST,"\n\r -Nothing:"); addTableauDeCommande(6); int randomNbr = rand() % 4; switch(randomNbr) { case 1: debug(DEBUG_DIRECTIVE_TEST," Javance"); addTableauDeCommande(6); break; case 2: debug(DEBUG_DIRECTIVE_TEST," Je tourne a gauche"); addTableauDeCommande(4); break; case 3: debug(DEBUG_DIRECTIVE_TEST," Je tourne a droite"); addTableauDeCommande(5); break; default: debug(DEBUG_DIRECTIVE_TEST," Jattend"); break; } //addTableauDeCommande(6); }*/ #ifdef DEBUG_DIRECTIVE_TASK_PAUSE debug("\n\r press any 9 key"); for(int idCmd=0; idCmd != 9; idCmd = fgetc(pc) - '0'); #endif /////////////// // Mouvement // /////////////// } else { switch(tableauDeCommange[0]) { case 11: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_calibre"); ctrDesPattes->calibre(); break; case 2: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_moveUp"); ctrDesPattes->moveUp(); break; case 3: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_moveDown"); ctrDesPattes->moveDown(); break; case 4: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_turnLeft"); ctrDesPattes->turnLeft(); break; case 5: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_trunRight"); ctrDesPattes->trunRight(); break; case 6: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_moveFront"); ctrDesPattes->moveFront(); break; case 7: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_moveBack"); ctrDesPattes->moveBack(); break; /*case 8: ctrDesPattes->moveBack(); break;*/ case 0x81: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 1"); ctrDesPattes->repositioner(1); break; case 0x82: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 2"); ctrDesPattes->repositioner(2); break; case 0x83: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 3"); ctrDesPattes->repositioner(3); break; case 0x84: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 4"); ctrDesPattes->repositioner(4); break; case 0x85: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 5"); ctrDesPattes->repositioner(5); break; case 0x86: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 6"); ctrDesPattes->repositioner(6); break; case 0x87: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 7"); ctrDesPattes->repositioner(7); break; case 0x88: debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 8"); ctrDesPattes->repositioner(8); break; } debug(DEBUG_DIRECTIVE_TEST,"\n\r -TAB:"); for(int i =0; i<DIRECTIVE_TABLEAUDECOMMANDE_SIZE; i++) debug(DEBUG_DIRECTIVE_TEST,"[%02x]",tableauDeCommange[i]); for(int i =0; i<DIRECTIVE_TABLEAUDECOMMANDE_SIZE; i++) tableauDeCommange[i]= tableauDeCommange[i+1]; tableauDeCommange[DIRECTIVE_TABLEAUDECOMMANDE_SIZE-1] = 0; if(size_tableauDeCommange != 0) size_tableauDeCommange--; } } else { ctrDesPattes->exec(); } #endif //DEBUG_DIRECTIVE_LEG_DISABLE //////////////////// // Update memoire // //////////////////// //////////// // Autre? // //////////// //debug(DEBUG_DIRECTIVE_TEST,"\n\rOut task directive"); #ifdef DEBUG_BOOT_GRAPHICAL_INTERFACE #ifdef DBG_MAIN_DELAY_SEC debug("\n\r\n wait \n\r\n"); wait(DBG_MAIN_DELAY_SEC); #endif debug("\n\r---------------------"); #endif } double Directive::IRToCm (double miliVolt) { double result=0; //debug("\n\r double value: %02f",miliVolt); if(miliVolt < tabIR[0][1])result = tabIR[0][0]; else if(miliVolt > tabIR[11][1])result = tabIR[11][0]; for(int i =1; (result==0)&&(i<12); i++) { if((tabIR[i-1][1]<=miliVolt) && (tabIR[i][1]>miliVolt)) { result=tabIR[i-1][0]+((tabIR[i][0]-tabIR[i-1][0])*((tabIR[i-1][1]-miliVolt)/(tabIR[i-1][1]-tabIR[i][1]))); } } return result; } void Directive::addTableauDeCommande(unsigned char value) { if(size_tableauDeCommange!=DIRECTIVE_TABLEAUDECOMMANDE_SIZE) { tableauDeCommange[size_tableauDeCommange]=value; size_tableauDeCommange++; } } void Directive::updateModuleValue(void) { string flag,data; double tempValue=0; flag.clear(); data.clear(); flag.append(1,7); //debug("\n\n\r flag size:%i \n\r",flag.size()); if(m_CtrlBridge->send(m_capteurUltrasonic[0],flag,data)) { /*if(data[0] != 1)//patch? tempValue = data[0];*/ if(data.size() == 2) { tempValue = data[0]; //tempValue = (tempValue<<8) + data[1]; tempValue = (data[0]<<8) + data[1]; } else tempValue = data[0]; if(data.size() != 0) m_valueCapteurUltrasonic = m_valueCapteurUltrasonic+((tempValue -m_valueCapteurUltrasonic)/DIRECTIVE_ULTRASONIC_BUFFER_LENGHT); } #ifdef DEBUG_DIRECTIVE_UPDATE_MODULE debug("\n\r m_valueCapteurUltrasonic"); debug("\n\n\r data size:%i \n\r",data.size()); switch(data.size()) { case 1: debug("\n\r -data(value):%i ",data[0]); break; case 2: debug("\n\r -data(value):%i ",(data[0]<<8) + data[1]); break; } //debug("\n\r -data(value):%i ",data[0]); debug("\n\r -tempValue(value):%02f ",tempValue); #endif tempValue=0; flag.clear(); data.clear(); flag.append(1,7); //debug("\n\n\r flag size:%i \n\r",flag.size()); if(m_CtrlBridge->send(m_capteurIR[0],flag,data)) { if(data.size() == 2) { //tempValue = data[0]<<8; //debug("\n\n\r monI: %02f",tempValue); //debug("\n\n\r mondata: %i",data[0]<<8); //tempValue = (tempValue<<8) + data[1]; tempValue = (data[0]<<8) + data[1]; } else tempValue = data[0]; if(data.size() != 0) m_valueCapteurIR=m_valueCapteurIR+((tempValue-m_valueCapteurIR)/DIRECTIVE_IR_BUFFER_LENGHT); } #ifdef DEBUG_DIRECTIVE_UPDATE_MODULE debug("\n\r m_valueCapteurIR"); debug("\n\n\r data size:%i \n\r",data.size()); switch(data.size()) { case 1: debug("\n\r -data(value):%i ",data[0]); break; case 2: debug("\n\r -data(value):%i ",(data[0]<<8) + data[1]); break; } //debug("\n\r -data(value):%i ",data[0]); debug("\n\r -tempValue(value):%02f ",tempValue); #endif //debug("\n\n\r data size:%i \n\r",data.size()); // debug("\n\r -IR(value):%f ",m_valueCapteurIR); //for(int i=0; i<data.size();i++)debug("\n\r -data[%i]:%i",i,data[i]); flag.clear(); data.clear(); flag.append(1,7); //debug("\n\n\r flag size:%i \n\r",flag.size()); if(m_CtrlBridge->send(m_capteurProximiter[0],flag,data)) m_valueCapteurProximiter = data[0]; // debug("\n\n\r data size:%i \n\r",data.size()); // debug("\n\r -m_valueCapteurProximiter(value):%i ",m_valueCapteurProximiter); } void Directive::analiseMaze(void) { string mymap = myMaze->showMap(); #ifdef DEBUG_DIRECTIVE_LABYRINTH debug("\n\r ----------------analiseMaze--------------\n\r"); debug("\n\r Labyrinthe map: X:%02i Y:%02i \n\r",(signed char)(myMaze->getX()),(signed char)(myMaze->getY())); debug("Direction: "); switch(myMaze->getDirection()) { case UP: debug("Up \n\r"); break; case DOWN: debug("Down \n\r"); break; case LEFT: debug("Left \n\r"); break; case RIGHT: debug("Right \n\r"); break; } for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+6])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+3])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i])); debug("\n\r"); #endif #ifndef DEBUG_DIRECTIVE_LABYRINTH_USER_CTRL /*if((ultrasonicToInch(m_valueCapteurUltrasonic)< 1) && (IRToCm(ADCTomv(m_valueCapteurIR)) <= 80)) { //capteur ultrasson embrouiller/imprecis debug(DEBUG_DIRECTIVE_TEST,"\n\r -Capteur Ultrasson brouiller"); //addTableauDeCommande(6); } else if(ultrasonicToInch(m_valueCapteurUltrasonic) >=24) { //plus proche que 10pouce debug(DEBUG_DIRECTIVE_TEST,"\n\r -Objet proche 2pi"); areaLatt =2; areaVert =2; } else if(ultrasonicToInch(m_valueCapteurUltrasonic) >=12) { //plus proche que 10pouce debug(DEBUG_DIRECTIVE_TEST,"\n\r -Objet proche 2pi"); areaLatt =1; areaVert =1; } debug(DEBUG_DIRECTIVE_TEST,": %i",ultrasonicToInch(m_valueCapteurUltrasonic)); if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 60) { debug(DEBUG_DIRECTIVE_TEST,"\n\r -Confirmation IR"); areaVert =2; } else if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 30) { debug(DEBUG_DIRECTIVE_TEST,"\n\r -Confirmation IR"); areaVert =1; } debug(DEBUG_DIRECTIVE_TEST,": %i",IRToCm(ADCTomv(m_valueCapteurIR)));*///old one updateMaze(); #ifdef DEBUG_DIRECTIVE_LABYRINTH mymap = myMaze->showMap(); //char caseCaract = 0; debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+6])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+3])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i])); debug("\n\r"); debug("\n\r-------------------\n\r"); #endif } char Directive::checkOtherWay(char dir, bool checkExplorer) { char result = 0; char order[3]; bool fini = false; switch(rand() % 6) { case 0://[0],[1],[2] order[0] = 0; order[1] = 1; order[2] = 2; break; case 1://[0],[2],[1] order[0] = 0; order[1] = 2; order[2] = 1; break; case 2://[1],[0],[2] order[0] = 1; order[1] = 0; order[2] = 2; break; case 3://[2],[0],[1] order[0] = 2; order[1] = 0; order[2] = 1; break; case 4://[1],[2],[0] order[0] = 1; order[1] = 2; order[2] = 0; break; case 5://[2],[1],[0] order[0] = 2; order[1] = 1; order[2] = 0; break; } debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rresult: "); for(int i=0; (i<3)&&(!fini); i++) { switch(order[i]) { // inclure un état memoir affin que si il a le choix entre une case vide et une explorer il priorise la vide// pt faire de meme avec pas explorer case 0: result = myMaze->getC_ToLeft(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r getC_ToLeft: %i %i ****",result,myMaze->getC_ToLeft()); //if(((Labyrinthe::case_t)result != Labyrinthe::error)&&((Labyrinthe::case_t)result != Labyrinthe::mur) &&((checkExplorer)?((Labyrinthe::case_t)result != Labyrinthe::explorer):1) ) if(((Labyrinthe::case_t)result == Labyrinthe::vide)&&((Labyrinthe::case_t)result != Labyrinthe::mur) &&((checkExplorer)?((Labyrinthe::case_t)result != Labyrinthe::explorer):1) ) { fini = true; result = LEFT; } break; case 1: result = myMaze->getC_Backward(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r getC_Backward: %i %i ****",result,myMaze->getC_Backward()); if(/*((Labyrinthe::case_t)result != Labyrinthe::error)&&*/((Labyrinthe::case_t)result != Labyrinthe::mur)&&((checkExplorer)?((Labyrinthe::case_t)result != Labyrinthe::explorer):1)) { fini = true; result = DOWN; } break; case 2: result = myMaze->getC_ToRight(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r getC_ToRight: %i %i ****",result,myMaze->getC_ToRight()); if(/*((Labyrinthe::case_t)result != Labyrinthe::error)&&*/((Labyrinthe::case_t)result != Labyrinthe::mur)&&((checkExplorer)?((Labyrinthe::case_t)result != Labyrinthe::explorer):1)) { fini = true; result = RIGHT; } break; } } if(fini) { switch(dir) { /*case UP: break;*/ case DOWN: switch(result) { case LEFT: result = RIGHT; break; case DOWN: result = UP; break; case RIGHT: result = LEFT; break; } break; case LEFT: switch(result) { case LEFT: result = DOWN; break; case DOWN: result = RIGHT; break; case RIGHT: result = UP; break; } break; case RIGHT: switch(result) { case LEFT: result = UP; break; case DOWN: result = LEFT; break; case RIGHT: result = DOWN; break; } break; } } else result =0; return result; } void Directive::updateMaze(void) { char areaLatt =0; char areaVert =0; if((inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic))<= 1) /*&& (IRToCm(ADCTomv(m_valueCapteurIR)) <= 80)*/)//enlever parce que il est lock avec des limites { //capteur ultrasson embrouiller/imprecis debug(DEBUG_DIRECTIVE_TEST,"\n\r -Capteur Ultrasson brouiller"); //addTableauDeCommande(6); } else if((inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic))) <= DIRECTIVE_ULTRASONIC_LIMIT_SHORT) { //plus proche que 1 case #ifdef DEBUG_DIRECTIVE_UPDATE_MODULE debug("\n\r -Objet proche %02f <= %i cm",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)),DIRECTIVE_ULTRASONIC_LIMIT_SHORT); #endif areaLatt =0; if(IRToCm(ADCTomv(m_valueCapteurIR)) <= DIRECTIVE_IR_LIMIT_SHORT) { /* /---\ |?M?| //vert 0 |.X.| //latt 0 |...| \---/ */ areaVert = 0; } else { /* /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ */ areaVert = 1; } } else if(inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)) <= DIRECTIVE_ULTRASONIC_LIMIT_MIDDLE) { //plus proche que 2 case #ifdef DEBUG_DIRECTIVE_UPDATE_MODULE debug("\n\r -Objet proche %g <= %i cm",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)),DIRECTIVE_ULTRASONIC_LIMIT_MIDDLE); #endif //modifier vue que il ne detecte pas asser loin /* /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ */ areaLatt =0; areaVert =1; } else if(inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)) <= DIRECTIVE_ULTRASONIC_LIMIT_LONG) { //plus proche que 2 case #ifdef DEBUG_DIRECTIVE_UPDATE_MODULE debug("\n\r -Objet proche %g <= %i cm",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)),DIRECTIVE_ULTRASONIC_LIMIT_LONG); #endif //modifier vue que il ne detecte pas asser loin /* /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ */ areaLatt =0; areaVert =1; } else if(inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)) > DIRECTIVE_ULTRASONIC_LIMIT_LONG) { //plus proche que 2 case #ifdef DEBUG_DIRECTIVE_UPDATE_MODULE debug("\n\r -Objet proche %g >= %i cm",inchToCm(ultrasonicToInch(m_valueCapteurUltrasonic)),DIRECTIVE_ULTRASONIC_LIMIT_LONG); #endif //modifier vue que il ne detecte pas asser loin /* /?V?\ |?V?| //vert 2 |.X.| //latt 0 |...| // objet possiblement a proximiter \---/ */ areaLatt =0; areaVert =2; } //debug("\n\r areaLatt: %i, areaVert: %i",areaLatt,areaVert); #endif #ifdef DEBUG_DIRECTIVE_LABYRINTH_USER_CTRL debug("\n\r---analise maze(manualy CTRL)---\n\r"); int idCommand = -1; areaVert = 9; areaLatt = 9; /*do { debug("\n\r Enter areaVert [0,1,2]: "); idCommand = fgetc(pc) - '0'; debug(" %i",idCommand); } while((idCommand < 0) && (idCommand > 2)); areaVert = idCommand; do { debug("\n\r Enter areaLatt [0,1,2]: "); idCommand = fgetc(pc) - '0'; debug(" %i",idCommand); } while((idCommand < 0) && (idCommand > 2)); areaLatt = idCommand;*/ debug("\n\r"); #endif //changer lalgoritme ici /* /---\ |?M?| //vert 0 |.X.| //latt 0 |...| \---/ /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ /?M?\ |VVV| //vert 1 |.X.| //latt 1 |...| // objet possiblement a proximiter \---/ /?M?\ |VVV| //vert 1 |.X.| //latt 2 |...| // cas impossible \---/ /?V?\ |?V?| //vert 2 |.X.| //latt 0 |...| // objet possiblement a proximiter \---/ /?V?\ |VVV| //vert 2 |.X.| //latt 1 |...| // objet possiblement a proximiter \---/ /VVV\ |VVV| //vert 2 |.X.| //latt 2 |...| \---/ */ //vertical==0 latt==0 //mur devant //vertical==1 latt==1 #ifdef DEBUG_DIRECTIVE_UPDATE_MODULE debug("\n\r vertical:%i \n\r latt: %i",areaVert,areaLatt); #endif //////////////////////////////////// //345 //012 | numerotation des case /////////////////////////////////// signed char fieldX[6]= {0,0,0,0,0,0}; signed char fieldY[6]= {0,0,0,0,0,0}; bool fieldSet = false; switch(myMaze->getDirection()) { case UP: fieldX[0] = myMaze->getX()-1; fieldY[0] = myMaze->getY()+1; fieldX[1] = myMaze->getX(); fieldY[1] = myMaze->getY()+1; fieldX[2] = myMaze->getX()+1; fieldY[2] = myMaze->getY()+1; fieldX[3] = myMaze->getX()-1; fieldY[3] = myMaze->getY()+2; fieldX[4] = myMaze->getX(); fieldY[4] = myMaze->getY()+2; fieldX[5] = myMaze->getX()+1; fieldY[5] = myMaze->getY()+2; break; case DOWN: fieldX[0] = myMaze->getX()-1; fieldY[0] = myMaze->getY()-1; fieldX[1] = myMaze->getX(); fieldY[1] = myMaze->getY()-1; fieldX[2] = myMaze->getX()+1; fieldY[2] = myMaze->getY()-1; fieldX[3] = myMaze->getX()-1; fieldY[3] = myMaze->getY()-2; fieldX[4] = myMaze->getX(); fieldY[4] = myMaze->getY()-2; fieldX[5] = myMaze->getX()+1; fieldY[5] = myMaze->getY()-2; break; case LEFT: fieldX[0] = myMaze->getX()-1; fieldY[0] = myMaze->getY()-1; fieldX[1] = myMaze->getX()-1; fieldY[1] = myMaze->getY(); fieldX[2] = myMaze->getX()-1; fieldY[2] = myMaze->getY()+1; fieldX[3] = myMaze->getX()-2; fieldY[3] = myMaze->getY()-1; fieldX[4] = myMaze->getX()-2; fieldY[4] = myMaze->getY(); fieldX[5] = myMaze->getX()-2; fieldY[5] = myMaze->getY()+1; break; case RIGHT: fieldX[0] = myMaze->getX()+1; fieldY[0] = myMaze->getY()-1; fieldX[1] = myMaze->getX()+1; fieldY[1] = myMaze->getY(); fieldX[2] = myMaze->getX()+1; fieldY[2] = myMaze->getY()+1; fieldX[3] = myMaze->getX()+2; fieldY[3] = myMaze->getY()-1; fieldX[4] = myMaze->getX()+2; fieldY[4] = myMaze->getY(); fieldX[5] = myMaze->getX()+2; fieldY[5] = myMaze->getY()+1; break; } if(areaVert >= areaLatt) { switch(areaVert) { case 0: /* /---\ |?M?| //vert 0 |.X.| //latt 0 |...| \---/ */ myMaze->setC(Labyrinthe::mur,fieldX[1],fieldY[1]); fieldSet = true; break; case 1: /* /-M-\ |?V?| //vert 1 |.X.| //latt 0 |...| \---/ */ myMaze->setC(Labyrinthe::vide,fieldX[1],fieldY[1]); myMaze->setC(Labyrinthe::mur,fieldX[4],fieldY[4]); fieldSet = true; if(areaLatt == 1) { /* /?M?\ |VVV| //vert 1 |.X.| //latt 1 |...| // objet possiblement a proximiter \---/ */ myMaze->setC(Labyrinthe::vide,fieldX[0],fieldY[0]); myMaze->setC(Labyrinthe::vide,fieldX[2],fieldY[2]); } break; case 2: fieldSet = true; switch(areaLatt) { case 0: /* /?V?\ |?V?| //vert 2 |.X.| //latt 0 |...| // objet possiblement a proximiter \---/ */ myMaze->setC(Labyrinthe::vide,fieldX[1],fieldY[1]); myMaze->setC(Labyrinthe::vide,fieldX[4],fieldY[4]); break; case 1: /* /?V?\ |VVV| //vert 2 |.X.| //latt 1 |...| // objet possiblement a proximiter \---/ */ for(int i = 0; i<3; i++)myMaze->setC(Labyrinthe::vide,fieldX[i],fieldY[i]); myMaze->setC(Labyrinthe::vide,fieldX[4],fieldY[4]); break; case 2: /* /VVV\ |VVV| //vert 2 |.X.| //latt 2 |...| \---/ */ for(int i = 0; i<6; i++)myMaze->setC( Labyrinthe::vide,fieldX[i],fieldY[i]); break; } break; } } if(fieldSet == false) { // on a un cas impossible //myMaze->setC(Labyrinthe::error,fieldX[1],fieldY[1]); } //////////////////////////////////// } void Directive::checkOtherWay(char &caseFront, char &caseBack, char &caseLeft, char &caseRight) { caseFront = myMaze->getC_Foward(); caseBack = myMaze->getC_Backward(); caseRight = myMaze->getC_ToRight(); caseLeft = myMaze->getC_ToLeft(); } bool Directive::searchNewWay(void) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r ---- Search new way ----\n\r"); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Labyrinthe map: X:%02i Y:%02i \n\r",(signed char)(myMaze->getX()),(signed char)(myMaze->getY())); bool result=false; string backupMap = myMaze->getAllMap(); signed char backupPosX = myMaze->getX(); signed char backupPosY = myMaze->getY(); string searchMap = backupMap; char tempdir,nextCase=0; positionXY tempPosition; char backupDir = myMaze->getDirection(); bool noOtherWay = false;//si apres un moment tu est entourer de mur bool wrongWay = false; //si jamais vien un moment que tu est pris /doit rebrousser chemin //bool checkAutour = false; bufferNewWay.clear(); tempPosition.posX = myMaze->getX(); tempPosition.posY = myMaze->getY(); //bufferNewWay.push_back(tempPosition); // pas sur de lutiliter debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Debut Boucle"); //asdfasda// Ici voire pour pt amiliorer et cheker a gauche et a droite au cas ou tu verai une case vide parce que la tu cours direct dans un mur avant de tourner. while((!result)&&(!noOtherWay)) { // nextCase = 0; debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Cycle Boucle\n\r"); nextCase = myMaze->getC_Foward(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r nextCase: [%02x]",nextCase); switch(nextCase ) { case Labyrinthe::pasExplorer : case Labyrinthe::vide : debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r vide ou pas explorer: "); myMaze->moveFoward(); result = true; break; case Labyrinthe::explorer :// ici pt amiliorer //add position a la liste debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Explorer"); myMaze->moveFoward(); //myMaze->setMyPos(Labyrinthe::error); break; case Labyrinthe::mur: tempdir = rand() % 2; debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Mur: %i",tempdir); if(tempdir == 1) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r left right"); tempdir = myMaze->getC_ToLeft(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r nextCase: [%02x]",tempdir); if((Labyrinthe::case_t)tempdir != Labyrinthe::mur) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r turn Left"); myMaze->turnLeft(); //backupDir = myMaze->getDirection(); } else { tempdir = myMaze->getC_ToRight(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r nextCase: [%02x]",tempdir); if((Labyrinthe::case_t)tempdir != Labyrinthe::mur) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r turn Right"); myMaze->turnRight(); //backupDir = myMaze->getDirection(); } else { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r aucun coter"); wrongWay = true; // verifie dans la liste } } } else { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r right left"); tempdir = myMaze->getC_ToRight(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r nextCase: [%02x]",tempdir); if((Labyrinthe::case_t)tempdir != Labyrinthe::mur) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r turn Right"); myMaze->turnRight(); //backupDir = myMaze->getDirection(); } else { tempdir = myMaze->getC_ToLeft(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r nextCase: [%02x]",tempdir); if((Labyrinthe::case_t)tempdir != Labyrinthe::mur) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r turn Left"); myMaze->turnLeft(); //backupDir = myMaze->getDirection(); } else { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r aucun coter"); wrongWay = true; // verifie dans la liste } } /* if(wrongWay == false) myMaze->moveFoward();*/ } //myMaze->setMyPos(Labyrinthe::error); break; } tempPosition.posX = myMaze->getX(); tempPosition.posY = myMaze->getY(); tempPosition.direction = myMaze->getDirection(); /*debug("\n\rtempPosition.posX: %i",tempPosition.posX); debug("\n\rtempPosition.posY: %i",tempPosition.posY); debug("\n\rbufferNewWay.posX: %i",(bufferNewWay.back().posX)); debug("\n\rbufferNewWay.posY: %i",(bufferNewWay.back().posY));*/ if((bufferNewWay.empty())||(((bufferNewWay.back().posX) != tempPosition.posX) || ((bufferNewWay.back().posY) != tempPosition.posY))) { bufferNewWay.push_back(tempPosition); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r bufferNewWay: %i",bufferNewWay.size()); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posX: %i",tempPosition.posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posY: %i",tempPosition.posY); } else if((bufferNewWay.empty())&& ((backupPosX != myMaze->getX()) || (backupPosY != myMaze->getY())) ) { bufferNewWay.push_back(tempPosition); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r bufferNewWay: %i",bufferNewWay.size()); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posX: %i",tempPosition.posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posY: %i",tempPosition.posY); } else if(((bufferNewWay.back()).posX == tempPosition.posX) && ((bufferNewWay.back()).posY == tempPosition.posY) && ((bufferNewWay.back()).direction != tempPosition.direction))//ra fait un tours { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r pop back a cause dun tournie"); bufferNewWay.pop_back(); bufferNewWay.push_back(tempPosition); } else { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r buffer non adder"); } if(wrongWay) { char caseFront,caseBack,caseLeft,caseRight; char wallCount; char directive; bool haveValuesChange = false; bool caseVideTrouver = false; bool caseExplorerTrouver = false; debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\n\r wrong way patching"); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r bufferNewWay size: %i",bufferNewWay.size()); std::list<positionXY>::iterator it; do { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r --wrong way boucle"); //bufferNewWay.pop_back(); if(!bufferNewWay.empty()) { directive = 0;//defenir fait rien it=bufferNewWay.end(); haveValuesChange = false; caseVideTrouver = false; caseExplorerTrouver = false; tempPosition.posX = (bufferNewWay.back()).posX; tempPosition.posY = bufferNewWay.back().posY; myMaze->setX(bufferNewWay.back().posX); myMaze->setY(bufferNewWay.back().posY); wallCount = 0; checkOtherWay(caseFront,caseBack,caseLeft,caseRight); if((Labyrinthe::case_t)caseFront == Labyrinthe::mur)wallCount++; if((Labyrinthe::case_t)caseBack == Labyrinthe::mur)wallCount++; if((Labyrinthe::case_t)caseLeft == Labyrinthe::mur)wallCount++; if((Labyrinthe::case_t)caseRight == Labyrinthe::mur)wallCount++; if(wallCount == 3) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r PATH trouver "); myMaze->setC(Labyrinthe::mur,tempPosition.posX,tempPosition.posY); } debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r checkOtherWay front: %i, back: %i, LEFT: %i, RIGHT: %i",caseFront,caseBack,caseLeft,caseRight); //tempPosition.posX = (bufferNewWay.back()).posX; //tempPosition.posY = bufferNewWay.back().posY; /*if(((Labyrinthe::case_t)caseFront == Labyrinthe::vide) || ((Labyrinthe::case_t)caseBack == Labyrinthe::vide) || ((Labyrinthe::case_t)caseLeft == Labyrinthe::vide) || ((Labyrinthe::case_t)caseRight == Labyrinthe::vide)) { debug("\n\r Path vide trouver!!!"); caseVideTrouver = true; wrongWay = false; }*/ //else //{ it--; if(/*((Labyrinthe::case_t)caseFront != Labyrinthe::error) &&*/ ((Labyrinthe::case_t)caseFront != Labyrinthe::mur)) { // une anciene case est potentiele,emt bonne myMaze->getXY_Foward(tempPosition.posX,tempPosition.posY); if((Labyrinthe::case_t)caseFront == Labyrinthe::vide) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Path vide trouver!!!"); caseVideTrouver = true; haveValuesChange = true; directive = 1; wrongWay = false; //debug("\n\r moveFoward"); } if((!caseVideTrouver) && (!caseExplorerTrouver)) { caseExplorerTrouver = true; /*if(((*it).posX != tempPosition.posX) && ((*it).posY != tempPosition.posY)) { wrongWay = false; myMaze->moveFoward();//setDirection(UP); debug("\n\r moveFoward"); debug("\n\r chemin trouver"); haveValuesChange = true; } else if(bufferNewWay.size() == 1) {*/ // debug("\n\r Last thing in buffer"); //myMaze->moveFoward();//setDirection(UP); directive = 1;// move foward haveValuesChange = true; // debug("\n\r moveFoward"); } } if(/*((Labyrinthe::case_t)caseBack != Labyrinthe::error) &&*/ ((Labyrinthe::case_t)caseBack != Labyrinthe::mur)) { // une anciene case est potentiele,emt bonne myMaze->getXY_Backward(tempPosition.posX,tempPosition.posY); if((Labyrinthe::case_t)caseBack == Labyrinthe::vide) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Path vide trouver!!!"); caseVideTrouver = true; haveValuesChange = true; directive = 2; wrongWay = false; //debug("\n\r moveFoward"); } if((!caseVideTrouver) && (!caseExplorerTrouver)) { caseExplorerTrouver = true; /*if(((*it).posX != tempPosition.posX) && ((*it).posY != tempPosition.posY)) { wrongWay = false; myMaze->turnBack();//setDirection(DOWN); debug("\n\r turn Back"); debug("\n\r chemin trouver"); haveValuesChange = true; } else if(bufferNewWay.size() == 1) {*/ //debug("\n\r Last thing in buffer"); //myMaze->turnBack();//setDirection(DOWN); directive = 2; //debug("\n\r turn Back"); haveValuesChange = true; } debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r direction: %i",myMaze->getDirection()); } if(/*((Labyrinthe::case_t)caseLeft != Labyrinthe::error) &&*/ ((Labyrinthe::case_t)caseLeft != Labyrinthe::mur)) { // une anciene case est potentiele,emt bonne myMaze->getXY_Left(tempPosition.posX,tempPosition.posY); if((Labyrinthe::case_t)caseLeft == Labyrinthe::vide) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Path vide trouver!!!"); caseVideTrouver = true; haveValuesChange = true; directive = 3; wrongWay = false; //debug("\n\r moveFoward"); } if((!!caseVideTrouver) && (!caseExplorerTrouver)) { caseExplorerTrouver = true; /*if(((*it).posX != tempPosition.posX) && ((*it).posY != tempPosition.posY)) { wrongWay = false; myMaze->turnLeft();//setDirection(LEFT); debug("\n\r turn Left"); debug("\n\r chemin trouver"); haveValuesChange = true; } else if(bufferNewWay.size() == 1) {*/ //debug("\n\r Last thing in buffer"); //myMaze->turnLeft();//setDirection(LEFT); directive = 3; //debug("\n\r turn Left"); haveValuesChange = true; } } if(/*((Labyrinthe::case_t)caseRight != Labyrinthe::error) &&*/ ((Labyrinthe::case_t)caseRight != Labyrinthe::mur)) { // une anciene case est potentiele,emt bonne myMaze->getXY_Right(tempPosition.posX,tempPosition.posY); if((Labyrinthe::case_t)caseRight == Labyrinthe::vide) { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Path vide trouver!!!"); caseVideTrouver = true; haveValuesChange = true; directive = 4; wrongWay = false; //debug("\n\r moveFoward"); } if((!caseVideTrouver)&& (!caseExplorerTrouver)) { caseExplorerTrouver = true; /*if(((bufferNewWay.back()).posX != tempPosition.posX) && ((bufferNewWay.back()).posY != tempPosition.posY)) { wrongWay = false; myMaze->turnRight();//setDirection(RIGHT); debug("\n\r turn Right"); debug("\n\r chemin trouver"); haveValuesChange = true; } else if(bufferNewWay.size() == 1) {*/ //debug("\n\r Last thing in buffer"); //myMaze->turnRight();//setDirection(RIGHT); directive = 4; // debug("\n\r turn Right"); haveValuesChange = true; } } //} } else noOtherWay = true; if(haveValuesChange) { switch(directive) { case 0: debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r doNothing"); break; case 1: debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r moveFoward"); myMaze->moveFoward(); break; case 2: debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r turn Back"); myMaze->turnBack(); break; case 3: debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r turn Left"); myMaze->turnLeft(); break; case 4: debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r turn Right"); myMaze->turnRight(); break; } tempPosition.posX = myMaze->getX(); tempPosition.posY = myMaze->getY(); tempPosition.direction = myMaze->getDirection(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r bufferNewWay: %i",bufferNewWay.size()); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posX: %i",tempPosition.posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posY: %i",tempPosition.posY); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.direction: %i",tempPosition.direction); for (std::list<positionXY>::iterator it=bufferNewWay.begin(),itEnd;( it != bufferNewWay.end()) && (!bufferNewWay.empty()); ++it) { //tempPosition = *(it); itEnd = bufferNewWay.end(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Bouble de meme position"); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posX: %i",(*it).posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posY: %i",(*it).posY); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.direction: %i",(*it).direction); if(((*it).posX == tempPosition.posX) && ((*it).posY == tempPosition.posY) && ((*it).direction == tempPosition.direction)) { while(((*it).posX != (*itEnd).posX) && ((*it).posY != (*itEnd).posY) && ((*it).direction != (*itEnd).direction) && (!bufferNewWay.empty())) { itEnd = bufferNewWay.end(); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r pop back et enmurer"); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r tempPosition.posX: %i",(bufferNewWay.back()).posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r tempPosition.posY: %i",(bufferNewWay.back()).posY); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r tempPosition.direction: %i",(bufferNewWay.back()).direction); myMaze->setC(Labyrinthe::mur,(bufferNewWay.back()).posX,(bufferNewWay.back()).posY); bufferNewWay.pop_back(); } //it--; noOtherWay = true;//placer ici pour patcher vue que je suis taner //checkAutour = true; } } if(((bufferNewWay.back()).posX == tempPosition.posX) && ((bufferNewWay.back()).posY == tempPosition.posY) && ((bufferNewWay.back()).direction != tempPosition.direction))//ra fait un tours { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r pop back a cause dun tournie"); bufferNewWay.pop_back(); } bufferNewWay.push_back(tempPosition); //bufferNewWay.pop_back();//on enleve pour pouvoir lire le dernier } else { debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r pop back"); bufferNewWay.pop_back();//on enleve pour pouvoir lire le dernier } #ifdef DEBUG_DIRECTIVE_LABYRINTH string mymap = myMaze->showMap(); debug("\n\r Labyrinthe map: X:%02i Y:%02i \n\r",(signed char)(myMaze->getX()),(signed char)(myMaze->getY())); //char caseCaract = 0; debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r direction: %i",myMaze->getDirection()); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+6])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i+3])); debug("\n\r"); for(int i=0; i<3; i++) debug(" [%c] ",myMaze->caseToChar((Labyrinthe::case_t)mymap[i])); #endif #ifdef DEBUG_DIRECTIVE_TASK_PAUSE debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r press any key...\n\r"); fgetc(pc); #endif } while((wrongWay) && (!noOtherWay)); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r Fin Boucle"); } #ifdef DEBUG_DIRECTIVE_TASK_PAUSE debug("\n\r press any key...\n\r"); fgetc(pc); #endif } myMaze->setX(backupPosX); myMaze->setY(backupPosY); myMaze->setAllMap(backupMap); myMaze->setDirection(backupDir); if(noOtherWay) { result = false; myMaze->setAllMap(""); myMaze->setX(0); myMaze->setY(0); myMaze->setDirection(backupDir); } debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\n\r dans le buffer"); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r bufferNewWay: %i",bufferNewWay.size()); for (std::list<positionXY>::iterator it=bufferNewWay.begin(); it != bufferNewWay.end(); ++it) { //tempPosition = *(it); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posX: %i",(*it).posX); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.posY: %i",(*it).posY); debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\rtempPosition.direction: %i",(*it).direction); } debug(DEBUG_DIRECTIVE_MAZE_GRAPHICAL_INTERFACE_ENABLE,"\n\r ----------------"); return result; } bool Directive::isAnotherWay(void) { bool result=false; return result; } void Directive::turnRightDirection(char currentDir, char nextDir) { #ifndef DEBUG_DIRECTIVE_LEG_DISABLE if(((nextDir + currentDir) == (UP + DOWN)) || ((nextDir + currentDir) == (LEFT + RIGHT))) { if(rand()%2) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_LEFT); for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_LEFT); //addTableauDeCommande(TBL_CMD_TURN_LEFT); } else { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_RIGHT); for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_RIGHT); //addTableauDeCommande(TBL_CMD_TURN_RIGHT); } } else { switch(currentDir) { case UP: if(nextDir == LEFT) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_LEFT); } else if(nextDir == RIGHT) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_RIGHT); } break; case DOWN: if(nextDir == LEFT) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_RIGHT); } else if(nextDir == LEFT) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_LEFT); } break; case LEFT: if(nextDir == UP) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_RIGHT); } else if(nextDir == DOWN) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_LEFT); } break; case RIGHT: if(nextDir == UP) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_LEFT); } else if(nextDir == DOWN) { for(int loop=0; loop < DIRECTIVE_NUMBER_TURN_90_DEG; loop++) addTableauDeCommande(TBL_CMD_TURN_RIGHT); } break; } } #endif }