![](/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:
- 37:f53e9f7a14df
- Parent:
- 36:474d4795aafa
- Child:
- 38:57d5f36a0333
diff -r 474d4795aafa -r f53e9f7a14df labyrinthe.cpp --- a/labyrinthe.cpp Sun Sep 13 06:56:09 2015 +0000 +++ b/labyrinthe.cpp Tue Sep 29 17:23:25 2015 +0000 @@ -23,27 +23,30 @@ { } -coordonerr Labyrinthe::getCoordoner(char &x, char &y) +//modifie X et Y pour metre le bon formatage des coordoner et sort l'ID de la case +//tout ca est qu'un formatage il ne garanti pas que la tuile existe +coordonerr Labyrinthe::getCoordoner(signed char &x, signed char &y) { coordonerr result = unset; - char newPosX,newPosY; + signed char newPosX = (signed char)x; + signed char newPosY = (signed char)y; #ifdef DEGUG_LABIRINTH_GET_COORDONER debug("\n\r----------getCoordoner------\n\r"); - debug(" x: %i Y: %i\n\r",x,y); + debug(" x: %i Y: %i\n\r",(signed char)x, (signed char)newPosY); #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); + newPosX = 3*((((signed char)newPosX<0)? (signed char)newPosX-2:(signed char)newPosX)/3); // je veux le nobre de fois quil a fait un tours et donc seulment les entier + newPosY = 3*((((signed char)newPosY<0)? (signed char)newPosY-2:(signed char)newPosY)/3); #ifdef DEGUG_LABIRINTH_GET_COORDONER - debug(" newPosX: %i newPosY: %i\n\r",newPosX,newPosY); + debug(" newPosX: %i newPosY: %i\n\r",(signed char)newPosX,(signed char)newPosY); #endif - switch(x - newPosX){ + switch((signed char)x - (signed char)newPosX){ case 0: switch(y - newPosY){ case 0: @@ -51,11 +54,11 @@ break; case 1: - result=C2; + result=C4; break; case 2: - result=C3; + result=C7; break; } break; @@ -63,7 +66,7 @@ case 1: switch(y - newPosY){ case 0: - result=C4; + result=C2; break; case 1: @@ -71,7 +74,7 @@ break; case 2: - result=C6; + result=C8; break; } break; @@ -79,11 +82,11 @@ case 2: switch(y - newPosY){ case 0: - result=C7; + result=C3; break; case 1: - result=C8; + result=C6; break; case 2: @@ -93,7 +96,7 @@ break; } - x = newPosX; + x = (signed char)newPosX; y = newPosY; /*result =(coordonerr)( 2 + newPosX); @@ -111,10 +114,10 @@ break; }*/ #ifdef DEGUG_LABIRINTH_GET_COORDONER - debug(" newPosX: %i newPosY: %i\n\r",newPosX,newPosY); + //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"); + debug("--------------------\n\r"); #endif return result; @@ -122,8 +125,8 @@ coordonerr Labyrinthe::getCoordoner(void) { coordonerr result; - char newPosX = m_posX; - char newPosY = m_posY; + signed char newPosX = m_posX; + signed char newPosY = m_posY; result = getCoordoner(newPosX,newPosY); @@ -134,11 +137,14 @@ { return showMap(m_posX,m_posY); } -string Labyrinthe::showMap(char x, char y) +string Labyrinthe::showMap(signed char x, signed char y) { //bool result=false; string theMap; theMap.clear(); + #ifdef DEBUG_LABYRINTHE_SHOW_MAP + debug("\n\r----------showmap------\n\r"); + #endif /* char newPosX = x; char newPosY = y; @@ -171,11 +177,11 @@ theMap.append(1,((mapDuLabyrinthe[(stringPosition)+3]>>0)& 3)); } - #ifdef DEBUG_BOOT_GRAPHICAL_INTERFACE - debug("\n\r----------showmap------\n\r"); - debug(" map size: %i \n\r",mapDuLabyrinthe.size()); + #ifdef DEBUG_LABYRINTHE_SHOW_MAP + debug(" mapDuLabyrinthe size: %i \n\r",mapDuLabyrinthe.size()); debug(" stringPosition: %i \n\r",stringPosition); if(stringPosition != -1) { + debug(" theMap size: %i \n\r",theMap.size()); 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 @@ -198,13 +204,16 @@ { return updateMap(mapUpdated,m_posX,m_posY); } -bool Labyrinthe::updateMap(string mapUpdated,char x, char y) +bool Labyrinthe::updateMap(string mapUpdated,signed char x, signed char y) { + #ifdef DEBUG_LABYRINTHE_GRAPHICAL_INTERFACE + debug("\n\r----------updateMap------\n\r"); + #endif bool result = true; int stringPosition = searchCoord(x,y); if(stringPosition == -1) - result= false; + 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); @@ -212,24 +221,43 @@ mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xF3) | ((mapUpdated[3] &0x03)<<2); mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xFC) | ((mapUpdated[4] &0x03)<<0); - mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+3]) & 0x3F) | ((mapUpdated[5] &0x03)<<6); + mapDuLabyrinthe[(stringPosition)+3] = ((mapDuLabyrinthe[(stringPosition)+3]) & 0x3F) | ((mapUpdated[5] &0x03)<<6); - mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xCF) | ((mapUpdated[6] &0x03)<<4); - mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xF3) | ((mapUpdated[7] &0x03)<<2); - mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xFC) | ((mapUpdated[8] &0x03)<<0); + mapDuLabyrinthe[(stringPosition)+3] = ((mapDuLabyrinthe[(stringPosition)+3]) & 0xCF) | ((mapUpdated[6] &0x03)<<4); + mapDuLabyrinthe[(stringPosition)+3] = ((mapDuLabyrinthe[(stringPosition)+3]) & 0xF3) | ((mapUpdated[7] &0x03)<<2); + mapDuLabyrinthe[(stringPosition)+3] = ((mapDuLabyrinthe[(stringPosition)+3]) & 0xFC) | ((mapUpdated[8] &0x03)<<0); } - + + #ifdef DEBUG_LABYRINTHE_GRAPHICAL_INTERFACE + debug(" result: %i \n\r",result?1:0); + 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)); //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)); //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)); //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"); + #endif return result; } -bool Labyrinthe::addMap(char x, char y) +bool Labyrinthe::addMap(signed char x, signed char y) { bool result = true; x &= 0x7F; // on coupe le 8eme bit y &= 0x7F; // mapDuLabyrinthe.append(1,((x<<1) | (y>>6)) );//x7,x6,x5,x4,x3,x2,x1,y7 - mapDuLabyrinthe.append(1,y<<1); //y6,y5,y4,y3,y2,y1,[C1],[C1] + mapDuLabyrinthe.append(1,y<<2); //y6,y5,y4,y3,y2,y1,[C1],[C1] mapDuLabyrinthe.append(2,0); //add C2-C9 vide #ifdef DEBUG_LABYRINTHE_ADD_MAP @@ -237,6 +265,7 @@ debug("x = %i \n\r",x); debug("y = %i \n\r",y); debug("((x<<1) | (y>>6)) = %i \n\r",((x<<1) | (y>>6))); + debug("-------------\n\r"); #endif @@ -244,17 +273,19 @@ } //renvoi l'ID du 4 byte de la tuile dans le string -int Labyrinthe::searchCoord(char posX,char posY) +// renvoi -1 en cas d'erreur +int Labyrinthe::searchCoord(signed char posX,signed char posY) { bool result=false; - char newPosX = posX; - char newPosY = posY; - unsigned char templateX = 0; - unsigned char templateY = 0; + signed char newPosX = posX; + signed char newPosY = posY; + signed char templateX = 0; + signed char templateY = 0; int i=0; + //string theMap; //theMap.clear(); - debug(DEBUGLABSEARCHCOORD,"------Search coord------"); + debug(DEBUGLABSEARCHCOORD,"------Search coord------\n\r"); debug(DEBUGLABSEARCHCOORD," posX: %i posY: %i \n\r",posX,posY); @@ -266,9 +297,15 @@ for( ; ((i*DEFAULTLABLEIGHT) < mapDuLabyrinthe.size()) && (!result); i++) { templateX = (mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])>>1; - templateY = (((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])&1)<<7) | ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])>>2); + templateY = (((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])&1)<<6) + ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])>>2); + + //patch pour les nombre negatif vue que originelment on est sur un 7 bit + if(templateX & 0x40) templateX |= 0x80; + if(templateY & 0x40) templateY |= 0x80; + //templateY += ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])>>2); - + debug(DEBUGLABSEARCHCOORD," mapDuLabyrinthe par1: %i mapDuLabyrinthe part2: %i \n\r",mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)],mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1]); + debug(DEBUGLABSEARCHCOORD," templateX: %i templateY: %i \n\r",templateX,templateY); if((templateX == newPosX) && (templateY == newPosY)) { result=true; //theMap.append(1,((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])& 3)); @@ -279,7 +316,7 @@ 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,"--------------------"); + debug(DEBUGLABSEARCHCOORD,"--------------------\n\r"); return (result==false? -1:i*DEFAULTLABLEIGHT); } @@ -292,65 +329,76 @@ * @param y position Y. * @return */ -case_t Labyrinthe::getC(char x, char y) +case_t Labyrinthe::getC(signed char x, signed char y) { - char newPosX = x; - char newPosY = y; + signed char newPosX = x; + signed char newPosY = y; coordonerr maCoordoner; maCoordoner=getCoordoner(newPosX,newPosY); return getC(x, y, maCoordoner); } -case_t Labyrinthe::getC(char x, char y, coordonerr cX) +case_t Labyrinthe::getC(signed char x, signed char y, coordonerr cX) { - char myCoordoner = 0xFF; - string result = showMap(x,y); - if(result.size() != 0) { - myCoordoner= result[cX]; - } - return (case_t)myCoordoner; -} -void Labyrinthe::setC(case_t value, char x, char y, coordonerr cX) -{ - #ifdef DEGUG_LABIRINTH_SET_COORDONER - debug("------SetC coord------"); - #endif + case_t myCoordoner = error; string result = showMap(x,y); if(result.size() != 0) { - 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); + myCoordoner = case_t(result[cX]); + } + return myCoordoner; +} +void Labyrinthe::setC(case_t value, signed char x, signed char y, coordonerr cX) +{ + #ifdef DEGUG_LABIRINTH_SET_COORDONER + debug("------SetC coord------\n\r"); + 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"); + debug(" X:%i Y:%i \n\r",x,y); + #endif + + if(value != error){ + string result = showMap(x,y); + + if(result.size() == 0) {//no map found so we create new tuile + #ifdef DEGUG_LABIRINTH_SET_COORDONER + debug(" no tuile found\n\r"); #endif + signed char newPosX = x; + signed char newPosY = y; + + getCoordoner(newPosX,newPosY); + addMap(newPosX,newPosY); + result = showMap(x,y); + #ifdef DEGUG_LABIRINTH_SET_COORDONER + debug(" result: %i\n\r",result.size()); + #endif + } - updateMap(result,x,y); - + if(result.size() != 0){ #ifdef DEGUG_LABIRINTH_SET_COORDONER - debug("---------------------"); + debug(" tuile found\n\r"); #endif + result[cX-1]=value; + updateMap(result,x,y);//on remet les case modifier dans le string==map + } } + + #ifdef DEGUG_LABIRINTH_SET_COORDONER + debug("---------------------\n\r"); + #endif } void Labyrinthe::setC(case_t value, coordonerr cX) { setC(value,m_posX, m_posY,cX); } -void Labyrinthe::setC(case_t value, char x, char y) +void Labyrinthe::setC(case_t value, signed char x, signed char y) { - char newPosX = x; - char newPosY = y; + signed char newPosX = x; + signed char newPosY = y; coordonerr maCoordoner; maCoordoner=getCoordoner(newPosX,newPosY); - + debug("-setcase tempo newPosX: %i, x: %i\n\r",newPosX,x); setC(value,x,y,maCoordoner); } void Labyrinthe::setC_Up(case_t value)