![](/media/cache/profiles/4b0986230d9d7aded7f0c24033d221d4.jpg.50x50_q85.jpg)
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
Diff: labyrinthe.cpp
- Revision:
- 36:474d4795aafa
- Parent:
- 35:d81f6b927932
- Child:
- 37:f53e9f7a14df
diff -r d81f6b927932 -r 474d4795aafa labyrinthe.cpp --- a/labyrinthe.cpp Sat Sep 12 21:34:57 2015 +0000 +++ b/labyrinthe.cpp Sun Sep 13 06:56:09 2015 +0000 @@ -7,31 +7,95 @@ */ #include "labyrinthe.h" +typedef Labyrinthe::coordoner_t coordonerr; +typedef Labyrinthe::case_t case_t; Labyrinthe::Labyrinthe() { direction=0; mapDuLabyrinthe.clear(); addMap(0,0); - m_posX=0; - m_posY=0; + m_posX=1; + m_posY=1; } Labyrinthe::~Labyrinthe() { } -char Labyrinthe::getCoordoner(char &x, char &y) +coordonerr Labyrinthe::getCoordoner(char &x, char &y) { - char result; + coordonerr result = unset; char newPosX,newPosY; - newPosX = 3*(x/3); // je veux le nobre de fois quil a fait un tours et donc seulment les entier - newPosY = 3*(y/3); + + #ifdef DEGUG_LABIRINTH_GET_COORDONER + debug("\n\r----------getCoordoner------\n\r"); + debug(" x: %i Y: %i\n\r",x,y); + #endif + + + newPosX = 3*((((signed)x<0)? x-2:x)/3); // je veux le nobre de fois quil a fait un tours et donc seulment les entier + newPosY = 3*((((signed)y<0)? y-2:y)/3); + + #ifdef DEGUG_LABIRINTH_GET_COORDONER + debug(" newPosX: %i newPosY: %i\n\r",newPosX,newPosY); + #endif + + + switch(x - newPosX){ + case 0: + switch(y - newPosY){ + case 0: + result=C1; + break; + + case 1: + result=C2; + break; + + case 2: + result=C3; + break; + } + break; + + case 1: + switch(y - newPosY){ + case 0: + result=C4; + break; + + case 1: + result=C5; + break; + + case 2: + result=C6; + break; + } + break; + + case 2: + switch(y - newPosY){ + case 0: + result=C7; + break; + + case 1: + result=C8; + break; + + case 2: + result=C9; + break; + } + break; + } + x = newPosX; y = newPosY; - - result = 2 + newPosX; + /*result =(coordonerr)( 2 + newPosX); switch(newPosY) { case-1: @@ -45,42 +109,23 @@ case 1: result = result + (3*0); break; - } - - debug(DEBUGLABCOORD,"\n\r result: X: %02d Y:%02d",newPosX,newPosY); - debug(DEBUGLABCOORD,"\n\r result: :%02d \n\r",result); + }*/ + #ifdef DEGUG_LABIRINTH_GET_COORDONER + debug(" newPosX: %i newPosY: %i\n\r",newPosX,newPosY); + //debug("\n\r result: X: %02d Y:%02d",newPosX,newPosY); + debug("\n\r result: :%02d \n\r",result); + debug("\n\r--------------------\n\r"); + #endif return result; } -char Labyrinthe::getCoordoner(void) +coordonerr Labyrinthe::getCoordoner(void) { - char result=0; - char newPosX,newPosY; - - newPosX = 3*(m_posX/3); // je veux le nobre de fois quil a fait un tours et donc seulment les entier - newPosY = 3*(m_posY/3); - - newPosX = m_posX-newPosX; - newPosY = m_posY-newPosY; - - result = 2 + newPosX; - - switch(newPosY) { - case-1: - result = result + (3*2); - break; - - case 0: - result = result + (3*1); - break; - - case 1: - result = result + (3*0); - break; - } - - debug(DEBUGLABCOORD,"\n\r result: X: %02d Y:%02d",newPosX,newPosY); - debug(DEBUGLABCOORD,"\n\r result: :%02d \n\r",result); + coordonerr result; + char newPosX = m_posX; + char newPosY = m_posY; + + result = getCoordoner(newPosX,newPosY); return result; } @@ -131,17 +176,17 @@ debug(" map size: %i \n\r",mapDuLabyrinthe.size()); debug(" stringPosition: %i \n\r",stringPosition); if(stringPosition != -1) { - debug(" (mapDuLabyrinthe[(stringPosition)+1]>>0): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+1]>>0),((mapDuLabyrinthe[(stringPosition)+1]>>0)& 3)); - debug(" (mapDuLabyrinthe[(stringPosition)+2]>>6): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+2]>>6),((mapDuLabyrinthe[(stringPosition)+2]>>6)& 3)); - debug(" (mapDuLabyrinthe[(stringPosition)+2]>>4): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+2]>>4),((mapDuLabyrinthe[(stringPosition)+2]>>4)& 3)); + debug(" (mapDuLabyrinthe[(stringPosition)+1]>>0): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+1]>>0),((mapDuLabyrinthe[(stringPosition)+1]>>0)& 3)); //c1 + debug(" (mapDuLabyrinthe[(stringPosition)+2]>>6): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+2]>>6),((mapDuLabyrinthe[(stringPosition)+2]>>6)& 3)); //c2 + debug(" (mapDuLabyrinthe[(stringPosition)+2]>>4): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+2]>>4),((mapDuLabyrinthe[(stringPosition)+2]>>4)& 3)); //c3 debug(" \n\r"); - debug(" (mapDuLabyrinthe[(stringPosition)+2]>>2): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+2]>>2),((mapDuLabyrinthe[(stringPosition)+2]>>2)& 3)); - debug(" (mapDuLabyrinthe[(stringPosition)+2]>>0): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+2]>>0),((mapDuLabyrinthe[(stringPosition)+2]>>0)& 3)); - debug(" (mapDuLabyrinthe[(stringPosition)+3]>>6): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+3]>>6),((mapDuLabyrinthe[(stringPosition)+3]>>6)& 3)); + debug(" (mapDuLabyrinthe[(stringPosition)+2]>>2): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+2]>>2),((mapDuLabyrinthe[(stringPosition)+2]>>2)& 3)); //c4 + debug(" (mapDuLabyrinthe[(stringPosition)+2]>>0): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+2]>>0),((mapDuLabyrinthe[(stringPosition)+2]>>0)& 3)); //c5 + debug(" (mapDuLabyrinthe[(stringPosition)+3]>>6): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+3]>>6),((mapDuLabyrinthe[(stringPosition)+3]>>6)& 3)); //c6 debug(" \n\r"); - debug(" (mapDuLabyrinthe[(stringPosition)+3]>>4): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+3]>>4),((mapDuLabyrinthe[(stringPosition)+3]>>4)& 3)); - debug(" (mapDuLabyrinthe[(stringPosition)+3]>>2): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+3]>>2),((mapDuLabyrinthe[(stringPosition)+3]>>2)& 3)); - debug(" (mapDuLabyrinthe[(stringPosition)+3]>>0): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+3]>>0),((mapDuLabyrinthe[(stringPosition)+3]>>0)& 3)); + debug(" (mapDuLabyrinthe[(stringPosition)+3]>>4): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+3]>>4),((mapDuLabyrinthe[(stringPosition)+3]>>4)& 3)); //c7 + debug(" (mapDuLabyrinthe[(stringPosition)+3]>>2): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+3]>>2),((mapDuLabyrinthe[(stringPosition)+3]>>2)& 3)); //c8 + debug(" (mapDuLabyrinthe[(stringPosition)+3]>>0): %i &3== %i \n\r",(mapDuLabyrinthe[(stringPosition)+3]>>0),((mapDuLabyrinthe[(stringPosition)+3]>>0)& 3)); //c9 debug(" \n\r"); } debug("\n\r----------------------\n\r"); @@ -161,8 +206,8 @@ if(stringPosition == -1) result= false; else { - mapDuLabyrinthe[(stringPosition)+1] = ((mapDuLabyrinthe[(stringPosition)+1]) & 0xFC) | ((mapUpdated[0] &0x03)<<0); - mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0x3F) | ((mapUpdated[1] &0x03)<<6); + mapDuLabyrinthe[(stringPosition)+1] = ((mapDuLabyrinthe[(stringPosition)+1]) & 0xFC) | ((mapUpdated[0] &0x03)<<0); + mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0x3F) | ((mapUpdated[1] &0x03)<<6); mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xCF) | ((mapUpdated[2] &0x03)<<4); mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xF3) | ((mapUpdated[3] &0x03)<<2); @@ -198,99 +243,133 @@ return result; } +//renvoi l'ID du 4 byte de la tuile dans le string int Labyrinthe::searchCoord(char posX,char posY) { bool result=false; + char newPosX = posX; + char newPosY = posY; + unsigned char templateX = 0; + unsigned char templateY = 0; + int i=0; //string theMap; //theMap.clear(); debug(DEBUGLABSEARCHCOORD,"------Search coord------"); debug(DEBUGLABSEARCHCOORD," posX: %i posY: %i \n\r",posX,posY); - char newPosX = posX; - char newPosY = posY; - + getCoordoner(newPosX,newPosY); debug(DEBUGLABSEARCHCOORD," new posX: %i new posY: %i \n\r",newPosX,newPosY); - unsigned char templateX,templateY; - int i=0; + debug(DEBUGLABSEARCHCOORD," map size: %i \n\r",mapDuLabyrinthe.size()); + for( ; ((i*DEFAULTLABLEIGHT) < mapDuLabyrinthe.size()) && (!result); i++) { templateX = (mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])>>1; - templateY = ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])&1)<<7; - templateY += ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])>>2); + templateY = (((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])&1)<<7) | ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])>>2); + //templateY += ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])>>2); if((templateX == newPosX) && (templateY == newPosY)) { result=true; //theMap.append(1,((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])& 3)); } } + i--; debug(DEBUGLABSEARCHCOORD," templateX: %i \n\r",templateX); debug(DEBUGLABSEARCHCOORD," templateY: %i \n\r",templateY); + debug(DEBUGLABSEARCHCOORD," result: %i \n\r",result?1:0); + debug(DEBUGLABSEARCHCOORD," i*DEFAULTLABLEIGHT: %i \n\r",i*DEFAULTLABLEIGHT); debug(DEBUGLABSEARCHCOORD,"--------------------"); - return (i==0? -1:i*DEFAULTLABLEIGHT); + return (result==false? -1:i*DEFAULTLABLEIGHT); } -char Labyrinthe::getC(char cX) +case_t Labyrinthe::getC(coordonerr cX) { return getC(m_posX, m_posY, cX); } -char Labyrinthe::getC(char x, char y) +/** +* @param x position X. +* @param y position Y. +* @return +*/ +case_t Labyrinthe::getC(char x, char y) { char newPosX = x; char newPosY = y; - char maCoordoner; + coordonerr maCoordoner; maCoordoner=getCoordoner(newPosX,newPosY); return getC(x, y, maCoordoner); } -char Labyrinthe::getC(char x, char y, char cX) + +case_t Labyrinthe::getC(char x, char y, coordonerr cX) { char myCoordoner = 0xFF; string result = showMap(x,y); if(result.size() != 0) { myCoordoner= result[cX]; } - return myCoordoner; + return (case_t)myCoordoner; } -void Labyrinthe::setC(char value, char x, char y, char cX) +void Labyrinthe::setC(case_t value, char x, char y, coordonerr cX) { + #ifdef DEGUG_LABIRINTH_SET_COORDONER + debug("------SetC coord------"); + #endif string result = showMap(x,y); if(result.size() != 0) { - result[cX]=value; - updateMap(result,x,y); + result[cX-1]=value; + + + #ifdef DEGUG_LABIRINTH_SET_COORDONER + debug("\n\r Labyrinthe map: X:%02x Y:%02x \n\r",x,y); + debug("value: %i\n\r",value); + debug("coordoner: %i\n\r",cX); + for(int i=0; i<3; i++) debug(" [%02X] ",result[i+6]); + debug("\n\r"); + for(int i=0; i<3; i++) debug(" [%02X] ",result[i+3]); + debug("\n\r"); + for(int i=0; i<3; i++) debug(" [%02X] ",result[i]); + debug("\n\r"); + #endif + + updateMap(result,x,y); + + #ifdef DEGUG_LABIRINTH_SET_COORDONER + debug("---------------------"); + #endif } } -void Labyrinthe::setC(char value, char cX) +void Labyrinthe::setC(case_t value, coordonerr cX) { setC(value,m_posX, m_posY,cX); } -void Labyrinthe::setC(char value, char x, char y) +void Labyrinthe::setC(case_t value, char x, char y) { char newPosX = x; char newPosY = y; - char maCoordoner; + coordonerr maCoordoner; maCoordoner=getCoordoner(newPosX,newPosY); setC(value,x,y,maCoordoner); } -void Labyrinthe::setC_Up(char value) +void Labyrinthe::setC_Up(case_t value) { setC(value,m_posX, m_posY+1); } -void Labyrinthe::setC_Down(char value) +void Labyrinthe::setC_Down(case_t value) { setC(value,m_posX, m_posY-1); } -void Labyrinthe::setC_Left(char value) +void Labyrinthe::setC_Left(case_t value) { setC(value,m_posX-1, m_posY); } -void Labyrinthe::setC_Right(char value) +void Labyrinthe::setC_Right(case_t value) { setC(value,m_posX+1, m_posY); } -void Labyrinthe::setMyPos(char value) +void Labyrinthe::setMyPos(case_t value) { setC(value, m_posX, m_posY); } \ No newline at end of file