asdf
Dependencies: L3GD20 LSM303DLHC mbed
Headers/Mapping.h
- Committer:
- goy5022
- Date:
- 2014-04-03
- Revision:
- 7:95ebadc83fc7
- Parent:
- 5:9e504a5a1f48
File content as of revision 7:95ebadc83fc7:
#ifndef MAPPING_H #define MAPPING_H const unsigned char EMPTY = 0x00; const unsigned char LEFT = 0x01; const unsigned char RIGHT = 0x02; const unsigned char BOTTOM = 0x04; const unsigned char TOP = 0x08; const unsigned char FULL = 0x0F; const unsigned char visited = 0x10; const unsigned char x = 0x20; const unsigned char y = 0x40; const unsigned char z = 0x80; const unsigned char all = 0xF0; enum ORIENTATION { NORTH, EAST, SOUTH, WEST }; //unsigned char MAP[16][16]; int cells_traveled = 0; /// X Y unsigned char MAP1[16][16] = { // Y ---> // 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //1 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //2 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //3 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //4 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //5 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //6 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //7 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //8 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //9 X {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //10 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //11 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //12 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //13 {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //14 {0xF3, 0xF5, 0xF0, 0xFC, 0xF6, 0xF7, 0xF7, 0xF5, 0xF0, 0xFC, 0xF0, 0xF0, 0xFC, 0xFC, 0xFE, 0xF7}, //15 {0xF5, 0xFC, 0xF4, 0xFC, 0xFC, 0xFE, 0xFD, 0xFC, 0xF4, 0xFC, 0xF6, 0xF5, 0xFC, 0xFC, 0xFC, 0xFE} //16 }; unsigned char MAP[16][16] = { // Y ---> // 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 {0x09, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x0A}, //1 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //2 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //3 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //4 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //5 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //6 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //7 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //8 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //9 | X {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //10 | {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //11| {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //12 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //13 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //14 {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //15 {0x07, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x06} //16 }; ORIENTATION orientation = NORTH; unsigned char Xpos; unsigned char Ypos; void initMapping() { for(int i = 0; i < 16; i++) for(int j = 0; j < 16; j++) MAP[i][j] = EMPTY; Xpos = 15; Ypos = 0; orientation = NORTH; } int getXpos() { return Xpos; } int getYpos() { return Ypos; } bool validDimen(unsigned char x, unsigned char y) { return ((unsigned char)0 <= x && x < (unsigned char)16 && (unsigned char)0 <= y && y < (unsigned char)16); } bool getTopWall(unsigned char x, unsigned char y) { return MAP[x][y] & TOP; } bool getBottomWall(unsigned char x, unsigned char y) { return MAP[x][y] & BOTTOM; } bool getLeftWall(unsigned char x, unsigned char y) { return MAP[x][y] & LEFT; } bool getRightWall(unsigned char x, unsigned char y) { return MAP[x][y] & RIGHT; } void setTopWall(unsigned char x, unsigned char y, bool wall) { if(validDimen(x, y)) if(wall) MAP[x][y] |= TOP; else MAP[x][y] &= ~TOP; } void setBottomWall(unsigned char x, unsigned char y, bool wall) { if(validDimen(x, y)) if(wall) MAP[x][y] |= BOTTOM; else MAP[x][y] &= ~BOTTOM; } void setLeftWall(unsigned char x, unsigned char y, bool wall) { if(validDimen(x, y)) if(wall) MAP[x][y] |= LEFT; else MAP[x][y] &= ~LEFT; } void setRightWall(unsigned char x, unsigned char y, bool wall) { if(validDimen(x, y)) if(wall) MAP[x][y] |= RIGHT; else MAP[x][y] &= ~RIGHT; } //done void orientation_turnAround() { switch(orientation) { case NORTH: orientation = SOUTH; break; case WEST: orientation = EAST; break; case SOUTH: orientation = NORTH; break; case EAST: orientation = WEST; break; } } void orientation_turnLeft() { switch(orientation) { case NORTH: orientation = WEST; break; case WEST: orientation = SOUTH; break; case SOUTH: orientation = EAST; break; case EAST: orientation = NORTH; break; } } //done void orientation_turnRight() { switch(orientation) { case NORTH: orientation = EAST; break; case WEST: orientation = NORTH; break; case SOUTH: orientation = WEST; break; case EAST: orientation = SOUTH; break; } } //DONE void setFront(unsigned char x, unsigned char y, bool w) { switch(orientation) { case NORTH: setTopWall(x, y, w); setBottomWall(x - 1, y, w); break; case EAST: setRightWall(x, y, w); setLeftWall(x, y + 1, w); break; case WEST: setLeftWall(x, y, w); setRightWall(x, y - 1, w); break; case SOUTH: setBottomWall(x, y, w); setTopWall(x + 1, y, w); break; } } //DONE void setRight(unsigned char x, unsigned char y, bool w) { switch(orientation) { case WEST: setTopWall(x, y, w); setBottomWall(x - 1, y, w); break; case NORTH: setRightWall(x, y, w); setLeftWall(x, y + 1, w); break; case SOUTH: setLeftWall(x, y, w); setRightWall(x, y - 1, w); break; case EAST: setBottomWall(x, y, w); setTopWall(x + 1, y, w); break; } } //DONE void setLeft(unsigned char x, unsigned char y, bool w) { switch(orientation) { case EAST: setTopWall(x, y, w); setBottomWall(x - 1, y, w); break; case SOUTH: setRightWall(x, y, w); setLeftWall(x, y + 1, w); break; case NORTH: setLeftWall(x, y, w); setRightWall(x, y - 1, w); break; case WEST: setBottomWall(x, y, w); setTopWall(x + 1, y, w); break; } } void setLeft(bool exists) { setLeft(Xpos, Ypos, exists); } void setRight(bool exists) { setRight(Xpos, Ypos, exists); } void setFront(bool exists) { setFront(Xpos, Ypos, exists); } void printMap(Serial out) { for(int i = 0; i < 16; i++) { for(int j = 0; j < 16; j++) { out.printf("%-2i ", MAP[i][j]); } out.printf("\r"); } out.printf("\n\r"); } float round(double r) { double f = floor(r); if(r > f + 0.5) r = ceil(r); else r = f; return r; } void updateMaze() { //int cells_traveled = (int)round(time/0.3351); while(cells_traveled > 1) { //increment position along direction setRight(true); setLeft(true); cells_traveled--; } //increment position along direction if(wallRight()) setRight(true); else setRight(false); if(wallLeft()) setLeft(true); else setLeft(false); if(wallFront()) setFront(true); else setFront(false); } #endif