Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/Mapping.h@5:9e504a5a1f48, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 18:47:30 2014 +0000
Revision:
5:9e504a5a1f48
Parent:
2:997f57aee3b7
Child:
7:95ebadc83fc7
pid working, wall detection working, stopping not perfect;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
goy5022 0:c2ec30f28676 1 #ifndef MAPPING_H
goy5022 0:c2ec30f28676 2 #define MAPPING_H
goy5022 0:c2ec30f28676 3
goy5022 0:c2ec30f28676 4 const unsigned char EMPTY = 0x00;
goy5022 0:c2ec30f28676 5 const unsigned char LEFT = 0x01;
goy5022 0:c2ec30f28676 6 const unsigned char RIGHT = 0x02;
goy5022 0:c2ec30f28676 7 const unsigned char BOTTOM = 0x04;
goy5022 0:c2ec30f28676 8 const unsigned char TOP = 0x08;
goy5022 0:c2ec30f28676 9 const unsigned char FULL = 0x0F;
goy5022 2:997f57aee3b7 10 const unsigned char visited = 0x10;
goy5022 2:997f57aee3b7 11 const unsigned char x = 0x20;
goy5022 2:997f57aee3b7 12 const unsigned char y = 0x40;
goy5022 2:997f57aee3b7 13 const unsigned char z = 0x80;
goy5022 2:997f57aee3b7 14 const unsigned char all = 0xF0;
goy5022 0:c2ec30f28676 15
goy5022 0:c2ec30f28676 16 enum ORIENTATION
goy5022 0:c2ec30f28676 17 {
goy5022 0:c2ec30f28676 18 NORTH,
goy5022 0:c2ec30f28676 19 EAST,
goy5022 0:c2ec30f28676 20 SOUTH,
goy5022 0:c2ec30f28676 21 WEST
goy5022 0:c2ec30f28676 22 };
goy5022 2:997f57aee3b7 23 //unsigned char MAP[16][16];
goy5022 0:c2ec30f28676 24
goy5022 2:997f57aee3b7 25 /// X Y
goy5022 2:997f57aee3b7 26 unsigned char MAP[16][16] = {
goy5022 2:997f57aee3b7 27
goy5022 2:997f57aee3b7 28 // Y --->
goy5022 2:997f57aee3b7 29 // 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
goy5022 2:997f57aee3b7 30 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //1
goy5022 2:997f57aee3b7 31 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //2
goy5022 2:997f57aee3b7 32 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //3
goy5022 2:997f57aee3b7 33 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //4
goy5022 2:997f57aee3b7 34 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //5
goy5022 2:997f57aee3b7 35 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //6
goy5022 2:997f57aee3b7 36 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //7
goy5022 2:997f57aee3b7 37 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //8
goy5022 2:997f57aee3b7 38 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //9 X
goy5022 2:997f57aee3b7 39 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //10
goy5022 2:997f57aee3b7 40 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //11
goy5022 2:997f57aee3b7 41 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //12
goy5022 2:997f57aee3b7 42 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //13
goy5022 2:997f57aee3b7 43 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //14
goy5022 2:997f57aee3b7 44 {0xF3, 0xF5, 0xF0, 0xFC, 0xF6, 0xF7, 0xF7, 0xF5, 0xF0, 0xFC, 0xF0, 0xF0, 0xFC, 0xFC, 0xFE, 0xF7}, //15
goy5022 2:997f57aee3b7 45 {0xF5, 0xFC, 0xF4, 0xFC, 0xFC, 0xFE, 0xFD, 0xFC, 0xF4, 0xFC, 0xF6, 0xF5, 0xFC, 0xFC, 0xFC, 0xFE} //16
goy5022 2:997f57aee3b7 46 };
goy5022 0:c2ec30f28676 47
goy5022 0:c2ec30f28676 48 ORIENTATION orientation;
goy5022 0:c2ec30f28676 49 unsigned char Xpos;
goy5022 0:c2ec30f28676 50 unsigned char Ypos;
goy5022 0:c2ec30f28676 51
goy5022 0:c2ec30f28676 52 void initMapping()
goy5022 0:c2ec30f28676 53 {
goy5022 0:c2ec30f28676 54 for(int i = 0; i < 16; i++)
goy5022 0:c2ec30f28676 55 for(int j = 0; j < 16; j++)
goy5022 2:997f57aee3b7 56 MAP[i][j] = EMPTY;
goy5022 0:c2ec30f28676 57
goy5022 2:997f57aee3b7 58 Xpos = 15; /// This might be 15 depending on how we start out
goy5022 0:c2ec30f28676 59 Ypos = 0;
goy5022 0:c2ec30f28676 60
goy5022 2:997f57aee3b7 61 orientation = NORTH;
goy5022 0:c2ec30f28676 62 }
goy5022 0:c2ec30f28676 63
goy5022 2:997f57aee3b7 64 int getXpos() { return Xpos; }
goy5022 2:997f57aee3b7 65
goy5022 2:997f57aee3b7 66 int getYpos() { return Ypos; }
goy5022 0:c2ec30f28676 67
goy5022 0:c2ec30f28676 68 bool validDimen(unsigned char x, unsigned char y)
goy5022 0:c2ec30f28676 69 {
goy5022 0:c2ec30f28676 70 return ((unsigned char)0 <= x && x < (unsigned char)16 && (unsigned char)0 <= y && y < (unsigned char)16);
goy5022 0:c2ec30f28676 71 }
goy5022 0:c2ec30f28676 72
goy5022 0:c2ec30f28676 73 bool getTopWall(unsigned char x, unsigned char y)
goy5022 0:c2ec30f28676 74 {
goy5022 0:c2ec30f28676 75 return MAP[x][y] & TOP;
goy5022 0:c2ec30f28676 76 }
goy5022 0:c2ec30f28676 77
goy5022 0:c2ec30f28676 78 bool getBottomWall(unsigned char x, unsigned char y)
goy5022 0:c2ec30f28676 79 {
goy5022 0:c2ec30f28676 80 return MAP[x][y] & BOTTOM;
goy5022 0:c2ec30f28676 81 }
goy5022 0:c2ec30f28676 82
goy5022 0:c2ec30f28676 83 bool getLeftWall(unsigned char x, unsigned char y)
goy5022 0:c2ec30f28676 84 {
goy5022 0:c2ec30f28676 85 return MAP[x][y] & LEFT;
goy5022 0:c2ec30f28676 86 }
goy5022 0:c2ec30f28676 87
goy5022 0:c2ec30f28676 88 bool getRightWall(unsigned char x, unsigned char y)
goy5022 0:c2ec30f28676 89 {
goy5022 0:c2ec30f28676 90 return MAP[x][y] & RIGHT;
goy5022 0:c2ec30f28676 91 }
goy5022 0:c2ec30f28676 92
goy5022 2:997f57aee3b7 93 void setTopWall(unsigned char x, unsigned char y, bool wall)
goy5022 0:c2ec30f28676 94 {
goy5022 0:c2ec30f28676 95 if(validDimen(x, y))
goy5022 2:997f57aee3b7 96 if(wall)
goy5022 2:997f57aee3b7 97 MAP[x][y] |= TOP;
goy5022 2:997f57aee3b7 98 else
goy5022 2:997f57aee3b7 99 MAP[x][y] &= ~TOP;
goy5022 0:c2ec30f28676 100 }
goy5022 0:c2ec30f28676 101
goy5022 2:997f57aee3b7 102 void setBottomWall(unsigned char x, unsigned char y, bool wall)
goy5022 0:c2ec30f28676 103 {
goy5022 0:c2ec30f28676 104 if(validDimen(x, y))
goy5022 2:997f57aee3b7 105 if(wall)
goy5022 2:997f57aee3b7 106 MAP[x][y] |= BOTTOM;
goy5022 2:997f57aee3b7 107 else
goy5022 2:997f57aee3b7 108 MAP[x][y] &= ~BOTTOM;
goy5022 0:c2ec30f28676 109 }
goy5022 0:c2ec30f28676 110
goy5022 2:997f57aee3b7 111 void setLeftWall(unsigned char x, unsigned char y, bool wall)
goy5022 0:c2ec30f28676 112 {
goy5022 0:c2ec30f28676 113 if(validDimen(x, y))
goy5022 2:997f57aee3b7 114 if(wall)
goy5022 2:997f57aee3b7 115 MAP[x][y] |= LEFT;
goy5022 2:997f57aee3b7 116 else
goy5022 2:997f57aee3b7 117 MAP[x][y] &= ~LEFT;
goy5022 0:c2ec30f28676 118 }
goy5022 0:c2ec30f28676 119
goy5022 2:997f57aee3b7 120 void setRightWall(unsigned char x, unsigned char y, bool wall)
goy5022 0:c2ec30f28676 121 {
goy5022 0:c2ec30f28676 122 if(validDimen(x, y))
goy5022 2:997f57aee3b7 123 if(wall)
goy5022 2:997f57aee3b7 124 MAP[x][y] |= RIGHT;
goy5022 2:997f57aee3b7 125 else
goy5022 2:997f57aee3b7 126 MAP[x][y] &= ~RIGHT;
goy5022 0:c2ec30f28676 127 }
goy5022 2:997f57aee3b7 128 //done
goy5022 0:c2ec30f28676 129 void orientation_turnAround()
goy5022 0:c2ec30f28676 130 {
goy5022 0:c2ec30f28676 131 switch(orientation)
goy5022 0:c2ec30f28676 132 {
goy5022 0:c2ec30f28676 133 case NORTH:
goy5022 0:c2ec30f28676 134 orientation = SOUTH;
goy5022 0:c2ec30f28676 135 break;
goy5022 0:c2ec30f28676 136 case WEST:
goy5022 0:c2ec30f28676 137 orientation = EAST;
goy5022 0:c2ec30f28676 138 break;
goy5022 0:c2ec30f28676 139 case SOUTH:
goy5022 0:c2ec30f28676 140 orientation = NORTH;
goy5022 0:c2ec30f28676 141 break;
goy5022 0:c2ec30f28676 142 case EAST:
goy5022 0:c2ec30f28676 143 orientation = WEST;
goy5022 0:c2ec30f28676 144 break;
goy5022 0:c2ec30f28676 145 }
goy5022 0:c2ec30f28676 146 }
goy5022 0:c2ec30f28676 147
goy5022 2:997f57aee3b7 148
goy5022 0:c2ec30f28676 149 void orientation_turnLeft()
goy5022 0:c2ec30f28676 150 {
goy5022 0:c2ec30f28676 151 switch(orientation)
goy5022 0:c2ec30f28676 152 {
goy5022 0:c2ec30f28676 153 case NORTH:
goy5022 0:c2ec30f28676 154 orientation = WEST;
goy5022 0:c2ec30f28676 155 break;
goy5022 0:c2ec30f28676 156 case WEST:
goy5022 0:c2ec30f28676 157 orientation = SOUTH;
goy5022 0:c2ec30f28676 158 break;
goy5022 0:c2ec30f28676 159 case SOUTH:
goy5022 0:c2ec30f28676 160 orientation = EAST;
goy5022 0:c2ec30f28676 161 break;
goy5022 0:c2ec30f28676 162 case EAST:
goy5022 0:c2ec30f28676 163 orientation = NORTH;
goy5022 0:c2ec30f28676 164 break;
goy5022 0:c2ec30f28676 165 }
goy5022 0:c2ec30f28676 166 }
goy5022 0:c2ec30f28676 167
goy5022 2:997f57aee3b7 168
goy5022 2:997f57aee3b7 169 //done
goy5022 0:c2ec30f28676 170 void orientation_turnRight()
goy5022 0:c2ec30f28676 171 {
goy5022 0:c2ec30f28676 172 switch(orientation)
goy5022 0:c2ec30f28676 173 {
goy5022 0:c2ec30f28676 174 case NORTH:
goy5022 0:c2ec30f28676 175 orientation = EAST;
goy5022 0:c2ec30f28676 176 break;
goy5022 0:c2ec30f28676 177 case WEST:
goy5022 0:c2ec30f28676 178 orientation = NORTH;
goy5022 0:c2ec30f28676 179 break;
goy5022 0:c2ec30f28676 180 case SOUTH:
goy5022 0:c2ec30f28676 181 orientation = WEST;
goy5022 0:c2ec30f28676 182 break;
goy5022 0:c2ec30f28676 183 case EAST:
goy5022 0:c2ec30f28676 184 orientation = SOUTH;
goy5022 0:c2ec30f28676 185 break;
goy5022 0:c2ec30f28676 186 }
goy5022 0:c2ec30f28676 187 }
goy5022 0:c2ec30f28676 188
goy5022 2:997f57aee3b7 189 //DONE
goy5022 2:997f57aee3b7 190 void setFront(unsigned char x, unsigned char y, bool w)
goy5022 0:c2ec30f28676 191 {
goy5022 0:c2ec30f28676 192 switch(orientation)
goy5022 0:c2ec30f28676 193 {
goy5022 0:c2ec30f28676 194 case NORTH:
goy5022 2:997f57aee3b7 195 setTopWall(x, y, w);
goy5022 2:997f57aee3b7 196 setBottomWall(x - 1, y, w);
goy5022 0:c2ec30f28676 197 break;
goy5022 0:c2ec30f28676 198 case EAST:
goy5022 2:997f57aee3b7 199 setRightWall(x, y, w);
goy5022 2:997f57aee3b7 200 setLeftWall(x, y + 1, w);
goy5022 0:c2ec30f28676 201 break;
goy5022 0:c2ec30f28676 202 case WEST:
goy5022 2:997f57aee3b7 203 setLeftWall(x, y, w);
goy5022 2:997f57aee3b7 204 setRightWall(x, y - 1, w);
goy5022 0:c2ec30f28676 205 break;
goy5022 0:c2ec30f28676 206 case SOUTH:
goy5022 2:997f57aee3b7 207 setBottomWall(x, y, w);
goy5022 2:997f57aee3b7 208 setTopWall(x + 1, y, w);
goy5022 0:c2ec30f28676 209 break;
goy5022 0:c2ec30f28676 210 }
goy5022 0:c2ec30f28676 211 }
goy5022 0:c2ec30f28676 212
goy5022 2:997f57aee3b7 213 //DONE
goy5022 2:997f57aee3b7 214 void setRight(unsigned char x, unsigned char y, bool w)
goy5022 0:c2ec30f28676 215 {
goy5022 0:c2ec30f28676 216 switch(orientation)
goy5022 0:c2ec30f28676 217 {
goy5022 0:c2ec30f28676 218 case WEST:
goy5022 2:997f57aee3b7 219 setTopWall(x, y, w);
goy5022 2:997f57aee3b7 220 setBottomWall(x - 1, y, w);
goy5022 0:c2ec30f28676 221 break;
goy5022 0:c2ec30f28676 222 case NORTH:
goy5022 2:997f57aee3b7 223 setRightWall(x, y, w);
goy5022 2:997f57aee3b7 224 setLeftWall(x, y + 1, w);
goy5022 0:c2ec30f28676 225 break;
goy5022 0:c2ec30f28676 226 case SOUTH:
goy5022 2:997f57aee3b7 227 setLeftWall(x, y, w);
goy5022 2:997f57aee3b7 228 setRightWall(x, y - 1, w);
goy5022 0:c2ec30f28676 229 break;
goy5022 0:c2ec30f28676 230 case EAST:
goy5022 2:997f57aee3b7 231 setBottomWall(x, y, w);
goy5022 2:997f57aee3b7 232 setTopWall(x + 1, y, w);
goy5022 0:c2ec30f28676 233 break;
goy5022 0:c2ec30f28676 234 }
goy5022 0:c2ec30f28676 235 }
goy5022 0:c2ec30f28676 236
goy5022 2:997f57aee3b7 237 //DONE
goy5022 2:997f57aee3b7 238 void setLeft(unsigned char x, unsigned char y, bool w)
goy5022 0:c2ec30f28676 239 {
goy5022 0:c2ec30f28676 240 switch(orientation)
goy5022 0:c2ec30f28676 241 {
goy5022 0:c2ec30f28676 242 case EAST:
goy5022 2:997f57aee3b7 243 setTopWall(x, y, w);
goy5022 2:997f57aee3b7 244 setBottomWall(x - 1, y, w);
goy5022 0:c2ec30f28676 245 break;
goy5022 0:c2ec30f28676 246 case SOUTH:
goy5022 2:997f57aee3b7 247 setRightWall(x, y, w);
goy5022 2:997f57aee3b7 248 setLeftWall(x, y + 1, w);
goy5022 0:c2ec30f28676 249 break;
goy5022 0:c2ec30f28676 250 case NORTH:
goy5022 2:997f57aee3b7 251 setLeftWall(x, y, w);
goy5022 2:997f57aee3b7 252 setRightWall(x, y - 1, w);
goy5022 0:c2ec30f28676 253 break;
goy5022 0:c2ec30f28676 254 case WEST:
goy5022 2:997f57aee3b7 255 setBottomWall(x, y, w);
goy5022 2:997f57aee3b7 256 setTopWall(x + 1, y, w);
goy5022 0:c2ec30f28676 257 break;
goy5022 0:c2ec30f28676 258 }
goy5022 0:c2ec30f28676 259 }
goy5022 0:c2ec30f28676 260
goy5022 2:997f57aee3b7 261 void setLeft(bool exists)
goy5022 0:c2ec30f28676 262 {
goy5022 2:997f57aee3b7 263 setLeft(Xpos, Ypos, exists);
goy5022 0:c2ec30f28676 264 }
goy5022 0:c2ec30f28676 265
goy5022 2:997f57aee3b7 266 void setRight(bool exists)
goy5022 0:c2ec30f28676 267 {
goy5022 2:997f57aee3b7 268 setRight(Xpos, Ypos, exists);
goy5022 0:c2ec30f28676 269 }
goy5022 0:c2ec30f28676 270
goy5022 2:997f57aee3b7 271 void setFront(bool exists)
goy5022 0:c2ec30f28676 272 {
goy5022 2:997f57aee3b7 273 setFront(Xpos, Ypos, exists);
goy5022 0:c2ec30f28676 274 }
goy5022 0:c2ec30f28676 275
goy5022 5:9e504a5a1f48 276 void printMap(Serial out)
goy5022 5:9e504a5a1f48 277 {
goy5022 5:9e504a5a1f48 278 for(int i = 0; i < 16; i++)
goy5022 5:9e504a5a1f48 279 {
goy5022 5:9e504a5a1f48 280 for(int j = 0; j < 16; j++)
goy5022 5:9e504a5a1f48 281 {
goy5022 5:9e504a5a1f48 282 out.printf("%-2i ", MAP[i][j]);
goy5022 5:9e504a5a1f48 283 }
goy5022 5:9e504a5a1f48 284
goy5022 5:9e504a5a1f48 285 out.printf("\r");
goy5022 5:9e504a5a1f48 286 }
goy5022 5:9e504a5a1f48 287 out.printf("\n\r");
goy5022 5:9e504a5a1f48 288 }
goy5022 5:9e504a5a1f48 289
goy5022 0:c2ec30f28676 290 #endif