Alexandre Salconi-Denis / Mbed 2 deprecated ProjetOctopode

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

labyrinthe.cpp@28:ac5c6350ed9a, 2015-07-28 (annotated)

Committer:
salco
Date:
Tue Jul 28 01:22:16 2015 +0000
Revision:
28:ac5c6350ed9a
Child:
35:d81f6b927932
tsest

Who changed what in which revision?

UserRevisionLine numberNew contents of line
salco 28:ac5c6350ed9a 1 #include "labyrinthe.h"
salco 28:ac5c6350ed9a 2
salco 28:ac5c6350ed9a 3
salco 28:ac5c6350ed9a 4 Labyrinthe::Labyrinthe()
salco 28:ac5c6350ed9a 5 {
salco 28:ac5c6350ed9a 6 direction=0;
salco 28:ac5c6350ed9a 7 mapDuLabyrinthe.clear();
salco 28:ac5c6350ed9a 8 addMap(0,0);
salco 28:ac5c6350ed9a 9 m_posX=0;
salco 28:ac5c6350ed9a 10 m_posY=0;
salco 28:ac5c6350ed9a 11 }
salco 28:ac5c6350ed9a 12
salco 28:ac5c6350ed9a 13 Labyrinthe::~Labyrinthe()
salco 28:ac5c6350ed9a 14 {
salco 28:ac5c6350ed9a 15 }
salco 28:ac5c6350ed9a 16
salco 28:ac5c6350ed9a 17 char Labyrinthe::getCoordoner(char &x, char &y)
salco 28:ac5c6350ed9a 18 {
salco 28:ac5c6350ed9a 19 char result;
salco 28:ac5c6350ed9a 20 char newPosX,newPosY;
salco 28:ac5c6350ed9a 21 newPosX = 3*(x/3); // je veux le nobre de fois quil a fait un tours et donc seulment les entier
salco 28:ac5c6350ed9a 22 newPosY = 3*(y/3);
salco 28:ac5c6350ed9a 23
salco 28:ac5c6350ed9a 24 x = newPosX;
salco 28:ac5c6350ed9a 25 y = newPosY;
salco 28:ac5c6350ed9a 26
salco 28:ac5c6350ed9a 27 result = 2 + newPosX;
salco 28:ac5c6350ed9a 28
salco 28:ac5c6350ed9a 29 switch(newPosY) {
salco 28:ac5c6350ed9a 30 case-1:
salco 28:ac5c6350ed9a 31 result = result + (3*2);
salco 28:ac5c6350ed9a 32 break;
salco 28:ac5c6350ed9a 33
salco 28:ac5c6350ed9a 34 case 0:
salco 28:ac5c6350ed9a 35 result = result + (3*1);
salco 28:ac5c6350ed9a 36 break;
salco 28:ac5c6350ed9a 37
salco 28:ac5c6350ed9a 38 case 1:
salco 28:ac5c6350ed9a 39 result = result + (3*0);
salco 28:ac5c6350ed9a 40 break;
salco 28:ac5c6350ed9a 41 }
salco 28:ac5c6350ed9a 42
salco 28:ac5c6350ed9a 43 debug(DEBUGLABCOORD,"\n\r result: X: %02d Y:%02d",newPosX,newPosY);
salco 28:ac5c6350ed9a 44 debug(DEBUGLABCOORD,"\n\r result: :%02d",result);
salco 28:ac5c6350ed9a 45
salco 28:ac5c6350ed9a 46 return result;
salco 28:ac5c6350ed9a 47 }
salco 28:ac5c6350ed9a 48 char Labyrinthe::getCoordoner(void)
salco 28:ac5c6350ed9a 49 {
salco 28:ac5c6350ed9a 50 char result=0;
salco 28:ac5c6350ed9a 51 char newPosX,newPosY;
salco 28:ac5c6350ed9a 52
salco 28:ac5c6350ed9a 53 newPosX = 3*(m_posX/3); // je veux le nobre de fois quil a fait un tours et donc seulment les entier
salco 28:ac5c6350ed9a 54 newPosY = 3*(m_posY/3);
salco 28:ac5c6350ed9a 55
salco 28:ac5c6350ed9a 56 newPosX = m_posX-newPosX;
salco 28:ac5c6350ed9a 57 newPosY = m_posY-newPosY;
salco 28:ac5c6350ed9a 58
salco 28:ac5c6350ed9a 59 result = 2 + newPosX;
salco 28:ac5c6350ed9a 60
salco 28:ac5c6350ed9a 61 switch(newPosY) {
salco 28:ac5c6350ed9a 62 case-1:
salco 28:ac5c6350ed9a 63 result = result + (3*2);
salco 28:ac5c6350ed9a 64 break;
salco 28:ac5c6350ed9a 65
salco 28:ac5c6350ed9a 66 case 0:
salco 28:ac5c6350ed9a 67 result = result + (3*1);
salco 28:ac5c6350ed9a 68 break;
salco 28:ac5c6350ed9a 69
salco 28:ac5c6350ed9a 70 case 1:
salco 28:ac5c6350ed9a 71 result = result + (3*0);
salco 28:ac5c6350ed9a 72 break;
salco 28:ac5c6350ed9a 73 }
salco 28:ac5c6350ed9a 74
salco 28:ac5c6350ed9a 75 debug(DEBUGLABCOORD,"\n\r result: X: %02d Y:%02d",newPosX,newPosY);
salco 28:ac5c6350ed9a 76 debug(DEBUGLABCOORD,"\n\r result: :%02d",result);
salco 28:ac5c6350ed9a 77
salco 28:ac5c6350ed9a 78 return result;
salco 28:ac5c6350ed9a 79 }
salco 28:ac5c6350ed9a 80
salco 28:ac5c6350ed9a 81 string Labyrinthe::showMap(void)
salco 28:ac5c6350ed9a 82 {
salco 28:ac5c6350ed9a 83 return showMap(m_posX,m_posY);
salco 28:ac5c6350ed9a 84 }
salco 28:ac5c6350ed9a 85 string Labyrinthe::showMap(char x, char y)
salco 28:ac5c6350ed9a 86 {
salco 28:ac5c6350ed9a 87 //bool result=false;
salco 28:ac5c6350ed9a 88 string theMap;
salco 28:ac5c6350ed9a 89 theMap.clear();
salco 28:ac5c6350ed9a 90 /* char newPosX = x;
salco 28:ac5c6350ed9a 91 char newPosY = y;
salco 28:ac5c6350ed9a 92
salco 28:ac5c6350ed9a 93 getCoordoner(newPosX,newPosY);
salco 28:ac5c6350ed9a 94 unsigned char templateX,templateY;
salco 28:ac5c6350ed9a 95 for(int i=0; ((i*DEFAULTLABLEIGHT)<mapDuLabyrinthe.size()) && (!result);i++)
salco 28:ac5c6350ed9a 96 {
salco 28:ac5c6350ed9a 97 templateX = (mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])>>1;
salco 28:ac5c6350ed9a 98 templateY = ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])&1)<<7;
salco 28:ac5c6350ed9a 99 templateY += ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])>>2);
salco 28:ac5c6350ed9a 100
salco 28:ac5c6350ed9a 101 if((templateX == newPosX) && (templateY == newPosY))
salco 28:ac5c6350ed9a 102 {
salco 28:ac5c6350ed9a 103 result=true;
salco 28:ac5c6350ed9a 104 theMap.append(1,((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])& 3));
salco 28:ac5c6350ed9a 105 }
salco 28:ac5c6350ed9a 106 }*/
salco 28:ac5c6350ed9a 107 int stringPosition = searchCoord(x,y);
salco 28:ac5c6350ed9a 108 if(stringPosition != -1) {
salco 28:ac5c6350ed9a 109 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+1]>>0)& 3));
salco 28:ac5c6350ed9a 110 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+2]>>6)& 3));
salco 28:ac5c6350ed9a 111 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+2]>>4)& 3));
salco 28:ac5c6350ed9a 112
salco 28:ac5c6350ed9a 113 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+2]>>2)& 3));
salco 28:ac5c6350ed9a 114 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+2]>>0)& 3));
salco 28:ac5c6350ed9a 115 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+3]>>6)& 3));
salco 28:ac5c6350ed9a 116
salco 28:ac5c6350ed9a 117 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+3]>>4)& 3));
salco 28:ac5c6350ed9a 118 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+3]>>2)& 3));
salco 28:ac5c6350ed9a 119 theMap.append(1,((mapDuLabyrinthe[(stringPosition)+3]>>0)& 3));
salco 28:ac5c6350ed9a 120 }
salco 28:ac5c6350ed9a 121 return theMap;
salco 28:ac5c6350ed9a 122 }
salco 28:ac5c6350ed9a 123
salco 28:ac5c6350ed9a 124 bool Labyrinthe::updateMap(string mapUpdated)
salco 28:ac5c6350ed9a 125 {
salco 28:ac5c6350ed9a 126 return updateMap(mapUpdated,m_posX,m_posY);
salco 28:ac5c6350ed9a 127 }
salco 28:ac5c6350ed9a 128 bool Labyrinthe::updateMap(string mapUpdated,char x, char y)
salco 28:ac5c6350ed9a 129 {
salco 28:ac5c6350ed9a 130 bool result = true;
salco 28:ac5c6350ed9a 131 int stringPosition = searchCoord(x,y);
salco 28:ac5c6350ed9a 132
salco 28:ac5c6350ed9a 133 if(stringPosition == -1)
salco 28:ac5c6350ed9a 134 result= false;
salco 28:ac5c6350ed9a 135 else {
salco 28:ac5c6350ed9a 136 mapDuLabyrinthe[(stringPosition)+1] = ((mapDuLabyrinthe[(stringPosition)+1]) & 0xFC) | ((mapUpdated[0] &0x03)<<0);
salco 28:ac5c6350ed9a 137 mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0x3F) | ((mapUpdated[1] &0x03)<<6);
salco 28:ac5c6350ed9a 138 mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xCF) | ((mapUpdated[2] &0x03)<<4);
salco 28:ac5c6350ed9a 139
salco 28:ac5c6350ed9a 140 mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xF3) | ((mapUpdated[3] &0x03)<<2);
salco 28:ac5c6350ed9a 141 mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xFC) | ((mapUpdated[4] &0x03)<<0);
salco 28:ac5c6350ed9a 142 mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+3]) & 0x3F) | ((mapUpdated[5] &0x03)<<6);
salco 28:ac5c6350ed9a 143
salco 28:ac5c6350ed9a 144 mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xCF) | ((mapUpdated[6] &0x03)<<4);
salco 28:ac5c6350ed9a 145 mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xF3) | ((mapUpdated[7] &0x03)<<2);
salco 28:ac5c6350ed9a 146 mapDuLabyrinthe[(stringPosition)+2] = ((mapDuLabyrinthe[(stringPosition)+2]) & 0xFC) | ((mapUpdated[8] &0x03)<<0);
salco 28:ac5c6350ed9a 147 }
salco 28:ac5c6350ed9a 148
salco 28:ac5c6350ed9a 149
salco 28:ac5c6350ed9a 150 return result;
salco 28:ac5c6350ed9a 151 }
salco 28:ac5c6350ed9a 152 bool Labyrinthe::addMap(char x, char y)
salco 28:ac5c6350ed9a 153 {
salco 28:ac5c6350ed9a 154 bool result = true;
salco 28:ac5c6350ed9a 155 x &= 0x7F; // on coupe le 8eme bit
salco 28:ac5c6350ed9a 156 y &= 0x7F;
salco 28:ac5c6350ed9a 157
salco 28:ac5c6350ed9a 158 mapDuLabyrinthe.append(1,((x<<1) | (y>>6)) );
salco 28:ac5c6350ed9a 159 mapDuLabyrinthe.append(1,y<<1);
salco 28:ac5c6350ed9a 160 mapDuLabyrinthe.append(2,0); // add C2-C9 vide
salco 28:ac5c6350ed9a 161
salco 28:ac5c6350ed9a 162 return result;
salco 28:ac5c6350ed9a 163 }
salco 28:ac5c6350ed9a 164
salco 28:ac5c6350ed9a 165 int Labyrinthe::searchCoord(char posX,char posY)
salco 28:ac5c6350ed9a 166 {
salco 28:ac5c6350ed9a 167 bool result=false;
salco 28:ac5c6350ed9a 168 //string theMap;
salco 28:ac5c6350ed9a 169 //theMap.clear();
salco 28:ac5c6350ed9a 170 char newPosX = posX;
salco 28:ac5c6350ed9a 171 char newPosY = posY;
salco 28:ac5c6350ed9a 172
salco 28:ac5c6350ed9a 173 getCoordoner(newPosX,newPosY);
salco 28:ac5c6350ed9a 174 unsigned char templateX,templateY;
salco 28:ac5c6350ed9a 175 int i=0;
salco 28:ac5c6350ed9a 176 for( ; ((i*DEFAULTLABLEIGHT)<mapDuLabyrinthe.size()) && (!result); i++) {
salco 28:ac5c6350ed9a 177 templateX = (mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])>>1;
salco 28:ac5c6350ed9a 178 templateY = ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)])&1)<<7;
salco 28:ac5c6350ed9a 179 templateY += ((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])>>2);
salco 28:ac5c6350ed9a 180
salco 28:ac5c6350ed9a 181 if((templateX == newPosX) && (templateY == newPosY)) {
salco 28:ac5c6350ed9a 182 result=true;
salco 28:ac5c6350ed9a 183 //theMap.append(1,((mapDuLabyrinthe[(i*DEFAULTLABLEIGHT)+1])& 3));
salco 28:ac5c6350ed9a 184 }
salco 28:ac5c6350ed9a 185 }
salco 28:ac5c6350ed9a 186 return (i==0? -1:i*DEFAULTLABLEIGHT);
salco 28:ac5c6350ed9a 187 }
salco 28:ac5c6350ed9a 188
salco 28:ac5c6350ed9a 189 char Labyrinthe::getC(char cX)
salco 28:ac5c6350ed9a 190 {
salco 28:ac5c6350ed9a 191 return getC(m_posX, m_posY, cX);
salco 28:ac5c6350ed9a 192 }
salco 28:ac5c6350ed9a 193 char Labyrinthe::getC(char x, char y)
salco 28:ac5c6350ed9a 194 {
salco 28:ac5c6350ed9a 195 char newPosX = x;
salco 28:ac5c6350ed9a 196 char newPosY = y;
salco 28:ac5c6350ed9a 197 char maCoordoner;
salco 28:ac5c6350ed9a 198 maCoordoner=getCoordoner(newPosX,newPosY);
salco 28:ac5c6350ed9a 199
salco 28:ac5c6350ed9a 200 return getC(x, y, maCoordoner);
salco 28:ac5c6350ed9a 201 }
salco 28:ac5c6350ed9a 202 char Labyrinthe::getC(char x, char y, char cX)
salco 28:ac5c6350ed9a 203 {
salco 28:ac5c6350ed9a 204 char myCoordoner = 0xFF;
salco 28:ac5c6350ed9a 205 string result = showMap(x,y);
salco 28:ac5c6350ed9a 206 if(result.size() != 0) {
salco 28:ac5c6350ed9a 207 myCoordoner= result[cX];
salco 28:ac5c6350ed9a 208 }
salco 28:ac5c6350ed9a 209 return myCoordoner;
salco 28:ac5c6350ed9a 210 }
salco 28:ac5c6350ed9a 211 void Labyrinthe::setC(char value, char x, char y, char cX)
salco 28:ac5c6350ed9a 212 {
salco 28:ac5c6350ed9a 213 string result = showMap(x,y);
salco 28:ac5c6350ed9a 214 if(result.size() != 0) {
salco 28:ac5c6350ed9a 215 result[cX]=value;
salco 28:ac5c6350ed9a 216 updateMap(result,x,y);
salco 28:ac5c6350ed9a 217 }
salco 28:ac5c6350ed9a 218 }
salco 28:ac5c6350ed9a 219 void Labyrinthe::setC(char value, char cX)
salco 28:ac5c6350ed9a 220 {
salco 28:ac5c6350ed9a 221 setC(value,m_posX, m_posY,cX);
salco 28:ac5c6350ed9a 222 }
salco 28:ac5c6350ed9a 223 void Labyrinthe::setC(char value, char x, char y)
salco 28:ac5c6350ed9a 224 {
salco 28:ac5c6350ed9a 225 char newPosX = x;
salco 28:ac5c6350ed9a 226 char newPosY = y;
salco 28:ac5c6350ed9a 227 char maCoordoner;
salco 28:ac5c6350ed9a 228 maCoordoner=getCoordoner(newPosX,newPosY);
salco 28:ac5c6350ed9a 229
salco 28:ac5c6350ed9a 230 setC(value,x,y,maCoordoner);
salco 28:ac5c6350ed9a 231 }
salco 28:ac5c6350ed9a 232 void Labyrinthe::setC_Up(char value)
salco 28:ac5c6350ed9a 233 {
salco 28:ac5c6350ed9a 234 setC(value,m_posX, m_posY+1);
salco 28:ac5c6350ed9a 235 }
salco 28:ac5c6350ed9a 236 void Labyrinthe::setC_Down(char value)
salco 28:ac5c6350ed9a 237 {
salco 28:ac5c6350ed9a 238 setC(value,m_posX, m_posY-1);
salco 28:ac5c6350ed9a 239 }
salco 28:ac5c6350ed9a 240 void Labyrinthe::setC_Left(char value)
salco 28:ac5c6350ed9a 241 {
salco 28:ac5c6350ed9a 242 setC(value,m_posX-1, m_posY);
salco 28:ac5c6350ed9a 243 }
salco 28:ac5c6350ed9a 244 void Labyrinthe::setC_Right(char value)
salco 28:ac5c6350ed9a 245 {
salco 28:ac5c6350ed9a 246 setC(value,m_posX+1, m_posY);
salco 28:ac5c6350ed9a 247 }