Mapping.h

00001 #ifndef MAPPING_H
00002 #define MAPPING_H
00003 
00004 const unsigned char EMPTY   = 0x00;
00005 const unsigned char LEFT    = 0x01;
00006 const unsigned char RIGHT   = 0x02;
00007 const unsigned char BOTTOM  = 0x04;
00008 const unsigned char TOP     = 0x08;
00009 const unsigned char FULL    = 0x0F;
00010 const unsigned char visited = 0x10;
00011 const unsigned char x       = 0x20;
00012 const unsigned char y       = 0x40;
00013 const unsigned char z       = 0x80; 
00014 const unsigned char all     = 0xF0;                           
00015 
00016 enum ORIENTATION
00017 {
00018     NORTH,
00019     EAST,
00020     SOUTH,
00021     WEST   
00022 };
00023 //unsigned char MAP[16][16];
00024 int cells_traveled = 0;
00025 ///                X  Y
00026 unsigned char MAP1[16][16] = {
00027     
00028 //                                        Y --->
00029 //  1     2     3     4     5     6     7     8     9    10    11    12    13    14    15    16                           
00030  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //1
00031  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //2
00032  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //3 
00033  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //4
00034  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //5
00035  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //6
00036  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //7
00037  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //8
00038  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //9                     X
00039  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //10
00040  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //11
00041  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //12
00042  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //13
00043  {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}, //14
00044  {0xF3, 0xF5, 0xF0, 0xFC, 0xF6, 0xF7, 0xF7, 0xF5, 0xF0, 0xFC, 0xF0, 0xF0, 0xFC, 0xFC, 0xFE, 0xF7}, //15
00045  {0xF5, 0xFC, 0xF4, 0xFC, 0xFC, 0xFE, 0xFD, 0xFC, 0xF4, 0xFC, 0xF6, 0xF5, 0xFC, 0xFC, 0xFC, 0xFE}  //16
00046 };
00047 
00048 unsigned char MAP[16][16] = {
00049     
00050 //                                        Y --->
00051 //  1    2     3     4     5     6     7     8     9    10    11    12    13    14    15    16                           
00052  {0x09, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x0A}, //1
00053  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //2
00054  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //3 
00055  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //4
00056  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //5
00057  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //6
00058  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //7
00059  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //8
00060  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //9         |            X
00061  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //10         |
00062  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //11|
00063  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //12
00064  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //13
00065  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //14
00066  {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02}, //15
00067  {0x07, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x04, 0x06}  //16
00068 };
00069 
00070 ORIENTATION orientation = NORTH;
00071 unsigned char Xpos;
00072 unsigned char Ypos;
00073 
00074 void initMapping()
00075 {
00076     for(int i = 0; i < 16; i++)
00077         for(int j = 0; j < 16; j++)
00078             MAP[i][j] = EMPTY;   
00079             
00080     Xpos = 15; 
00081     Ypos = 0;
00082     
00083     orientation = NORTH;
00084 }
00085 
00086 int getXpos() { return Xpos; }
00087 
00088 int getYpos() { return Ypos; }
00089 
00090 bool validDimen(unsigned char x, unsigned char y)
00091 {
00092     return ((unsigned char)0 <= x && x < (unsigned char)16 && (unsigned char)0 <= y && y < (unsigned char)16);
00093 }
00094 
00095 bool getTopWall(unsigned char x, unsigned char y)
00096 {
00097     return MAP[x][y] & TOP;
00098 }
00099 
00100 bool getBottomWall(unsigned char x, unsigned char y)
00101 {
00102     return MAP[x][y] & BOTTOM;
00103 }
00104 
00105 bool getLeftWall(unsigned char x, unsigned char y)
00106 {
00107     return MAP[x][y] & LEFT;
00108 }
00109 
00110 bool getRightWall(unsigned char x, unsigned char y)
00111 {
00112     return MAP[x][y] & RIGHT;
00113 }
00114 
00115 void setTopWall(unsigned char x, unsigned char y, bool wall)
00116 {
00117     if(validDimen(x, y))
00118         if(wall)
00119             MAP[x][y] |= TOP;            
00120         else
00121             MAP[x][y] &= ~TOP;
00122 }
00123 
00124 void setBottomWall(unsigned char x, unsigned char y, bool wall)
00125 {
00126     if(validDimen(x, y))
00127         if(wall)
00128             MAP[x][y] |= BOTTOM;            
00129         else
00130             MAP[x][y] &= ~BOTTOM;
00131 }
00132 
00133 void setLeftWall(unsigned char x, unsigned char y, bool wall)
00134 {
00135     if(validDimen(x, y))
00136         if(wall)
00137             MAP[x][y] |= LEFT;            
00138         else
00139             MAP[x][y] &= ~LEFT;
00140 }
00141 
00142 void setRightWall(unsigned char x, unsigned char y, bool wall)
00143 {
00144     if(validDimen(x, y))
00145         if(wall)
00146             MAP[x][y] |= RIGHT;            
00147         else
00148             MAP[x][y] &= ~RIGHT;
00149 }
00150 //done
00151 void orientation_turnAround()
00152 {
00153     switch(orientation)
00154     {
00155         case NORTH:
00156             orientation = SOUTH;
00157             break;
00158         case WEST:
00159             orientation = EAST;
00160             break;            
00161         case SOUTH:
00162             orientation = NORTH;
00163             break;            
00164         case EAST:
00165             orientation = WEST;
00166             break;            
00167     }
00168 }
00169 
00170 
00171 void orientation_turnLeft()
00172 {
00173     switch(orientation)
00174     {
00175         case NORTH:
00176             orientation = WEST;
00177             break;
00178         case WEST:
00179             orientation = SOUTH;
00180             break;            
00181         case SOUTH:
00182             orientation = EAST;
00183             break;            
00184         case EAST:
00185             orientation = NORTH;
00186             break;            
00187     }
00188 }
00189 
00190 
00191 //done
00192 void orientation_turnRight()
00193 {
00194     switch(orientation)
00195     {
00196         case NORTH:
00197             orientation = EAST;
00198             break;            
00199         case WEST:
00200             orientation = NORTH;
00201             break;            
00202         case SOUTH:
00203             orientation = WEST;
00204             break;
00205         case EAST:
00206             orientation = SOUTH;
00207             break;
00208     }
00209 }
00210 
00211 //DONE
00212 void setFront(unsigned char x, unsigned char y, bool w)
00213 {
00214     switch(orientation)
00215     {
00216         case NORTH:
00217             setTopWall(x, y, w);
00218             setBottomWall(x - 1, y, w);
00219             break;
00220         case EAST:
00221             setRightWall(x, y, w);        
00222             setLeftWall(x, y + 1, w);
00223             break;
00224         case WEST:
00225             setLeftWall(x, y, w);
00226             setRightWall(x, y - 1, w);
00227             break;
00228         case SOUTH:
00229             setBottomWall(x, y, w);
00230             setTopWall(x + 1, y, w);
00231             break;
00232     }
00233 }
00234 
00235 //DONE
00236 void setRight(unsigned char x, unsigned char y, bool w)
00237 {
00238     switch(orientation)
00239     {
00240         case WEST:
00241             setTopWall(x, y, w);
00242             setBottomWall(x - 1, y, w);
00243             break;
00244         case NORTH:
00245             setRightWall(x, y, w);        
00246             setLeftWall(x, y + 1, w);
00247             break;
00248         case SOUTH:
00249             setLeftWall(x, y, w);
00250             setRightWall(x, y - 1, w);
00251             break;
00252         case EAST:
00253             setBottomWall(x, y, w);
00254             setTopWall(x + 1, y, w);
00255             break;
00256     }
00257 }
00258 
00259 //DONE
00260 void setLeft(unsigned char x, unsigned char y, bool w)
00261 {
00262     switch(orientation)
00263     {
00264         case EAST:
00265             setTopWall(x, y, w);
00266             setBottomWall(x - 1, y, w);
00267             break;
00268         case SOUTH:
00269             setRightWall(x, y, w);        
00270             setLeftWall(x, y + 1, w);
00271             break;
00272         case NORTH:
00273             setLeftWall(x, y, w);
00274             setRightWall(x, y - 1, w);
00275             break;
00276         case WEST:
00277             setBottomWall(x, y, w);
00278             setTopWall(x + 1, y, w);
00279             break;
00280     }
00281 }
00282 
00283 void setLeft(bool exists)
00284 {
00285     setLeft(Xpos, Ypos, exists);   
00286 }
00287 
00288 void setRight(bool exists)
00289 {
00290     setRight(Xpos, Ypos, exists);   
00291 }
00292 
00293 void setFront(bool exists)
00294 {
00295     setFront(Xpos, Ypos, exists);   
00296 }
00297 
00298 void printMap(Serial out)
00299 {
00300     for(int i = 0; i < 16; i++)
00301     {
00302         for(int j = 0; j < 16; j++) 
00303         {
00304             out.printf("%-2i ", MAP[i][j]);
00305         }
00306         
00307         out.printf("\r");
00308     }   
00309     out.printf("\n\r");
00310 }
00311 
00312 float round(double r)
00313 {
00314     double f = floor(r);
00315     if(r > f + 0.5)
00316     r = ceil(r);
00317     else
00318     r = f;
00319     return r;
00320 }
00321 
00322 
00323 void updateMaze()
00324 {
00325     //int cells_traveled = (int)round(time/0.3351);
00326     while(cells_traveled > 1)
00327     {
00328        //increment position along direction
00329        setRight(true);
00330        setLeft(true);
00331        cells_traveled--;
00332     } 
00333     //increment position along direction   
00334     if(wallRight())
00335     setRight(true);
00336     else
00337     setRight(false);
00338     if(wallLeft())
00339     setLeft(true);
00340     else
00341     setLeft(false);
00342     if(wallFront())
00343     setFront(true);
00344     else
00345     setFront(false);
00346 }
00347 
00348 
00349 
00350 #endif