Programme de contrôle de l'octopode 4DOF, Theraphosa-Salconi.

Dependencies:   debug mbed

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
Revision:
37:f53e9f7a14df
Parent:
36:474d4795aafa
Child:
38:57d5f36a0333
--- 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)