maze.cpp

00001 /** maze_vt100 a simple maze using UniGraphic and MMA8451Q libraries
00002  */
00003 #include "mbed.h"
00004 #include "MMA8451Q.h"
00005 #include <ILI9341.h>
00006 #include "Arial12x12.h"
00007 #include "Arial24x23.h"
00008 #include "Arial28x28.h"
00009 #include "Arial43x48_numb.h"
00010 #include "maze.h"
00011 
00012 #define CELL_W  10
00013 #define CELL_H  10
00014 #define CELL_S   5
00015 #define CELL_R   4
00016 #define LEFT_OFFSET 20
00017 #define TOP_OFFSET 20
00018 
00019 #define DIR_STAY  0
00020 #define DIR_UP    1
00021 #define DIR_DOWN  2
00022 #define DIR_RIGHT 3 
00023 #define DIR_LEFT  4
00024 
00025 #define MMA8451_I2C_ADDRESS (0x1D<<1)
00026 
00027 typedef struct _pos {
00028     int x ;
00029     int y ;
00030 } pos_type ;
00031 
00032 extern MMA8451Q *acc ;
00033 extern ILI9341 TFT ;
00034 extern DigitalOut backlight ;
00035 float threshold = 0.2 ;
00036 
00037 /** Check if two pos_type values are same
00038  * @param pos_type a
00039  * @param pos_type b
00040  * @returns if a and b are same position
00041  */
00042 bool isSame(pos_type a, pos_type b)
00043 {
00044     return((a.x == b.x)&&(a.y == b.y)) ;
00045 }
00046 
00047 /** Draw the maze defined in the "maze.h"
00048  * @param pos_type *current actually the start point
00049  * @param pos_type *goal the position of the goal
00050  * @note those params are actually returned by this function
00051  */
00052 void drawTFTMaze(pos_type *current, pos_type *goal)
00053 {
00054     int x, y, i, j ;
00055     TFT.BusEnable(true) ;
00056     
00057     backlight = 0 ;
00058     TFT.foreground(White) ;
00059     TFT.background(Black) ;
00060     TFT.cls() ;
00061     for (j = 0 ; j < MAZE_H ; j++ ) {
00062         for (i = 0 ; i < MAZE_W ; i++ ) {
00063             x = i * CELL_W + LEFT_OFFSET ;
00064             y = j * CELL_H + TOP_OFFSET ;
00065             switch(maze[j][i]) {
00066             case 0: // path
00067                 TFT.fillrect(x, y, x+CELL_W-1, y+CELL_H-1, Black) ;
00068                 break ;
00069             case 1: // wall
00070                 TFT.fillrect(x, y, x+CELL_W-1, y+CELL_H-1, DarkGrey) ;
00071                 break ;
00072             case 2: // Start point
00073                 TFT.fillcircle(x+CELL_S, y+CELL_S, CELL_R, Green) ;
00074                 current->x = i ;
00075                 current->y = j ;
00076                 break ;
00077             case 3: // Goal
00078                 TFT.fillcircle(x+CELL_S, y+CELL_S, CELL_R, Red) ;
00079                 goal->x = i ;
00080                 goal->y = j ;
00081                 break ;
00082             default: // should not be here
00083                 break ;
00084             }
00085         }
00086     }
00087     backlight = 1 ;
00088     TFT.BusEnable(false) ;
00089 }
00090 
00091 /** Filter out too little move 
00092  * @param float in returned value from the acc
00093  * @returns float result filtered value of in
00094  */
00095 float filterVal(float in)
00096 {
00097     float result = 0.0 ;
00098     if ((-threshold > in)||(in > threshold)) {
00099         result = in ;
00100     }
00101     return( result ) ;
00102 }
00103 
00104 /** Decide which direction to go
00105  * @param float res[] acc value of x, y 
00106  * @returns int direction to move 
00107  */
00108 int getDirection(float res[])
00109 {
00110     float dx, dy ;
00111     int direction = DIR_STAY ;
00112     dx = filterVal(res[0]) ;
00113     dy = filterVal(res[1]) ;
00114 
00115     if ((dx*dx) > (dy*dy)) { // holizontal move
00116         if (dx > 0.0) {
00117             direction = DIR_DOWN ;
00118         } else if (dx < 0.0) {
00119             direction = DIR_UP ;
00120         }
00121     } else { // vertical move
00122         if (dy > 0.0) {
00123             direction = DIR_RIGHT ;
00124         } else if (dy < 0.0) {
00125             direction = DIR_LEFT ;
00126         }
00127     }
00128     return(direction) ;
00129 }
00130 
00131 /** Get next positon to move to
00132  * @param pos_type current where we are now
00133  * @param int direction which way we'd like to move
00134  * @returns the candidate positon for the next move
00135  */
00136 pos_type getNext(pos_type current, int direction) 
00137 {
00138     pos_type next = current ;
00139     switch(direction) {
00140     case DIR_STAY: 
00141         break ;
00142     case DIR_UP: 
00143         if (next.y > 0) { 
00144             next.y-- ; 
00145         }  
00146         break ;
00147     case DIR_DOWN: 
00148         if (next.y < (MAZE_H - 1)) { 
00149             next.y++ ; 
00150         } 
00151         break ;
00152     case DIR_RIGHT: 
00153         if (next.x < (MAZE_W - 1)) { 
00154             next.x++ ; 
00155         } 
00156         break ;
00157     case DIR_LEFT: 
00158         if (next.x > 0) { 
00159             next.x-- ; 
00160         } 
00161         break ;
00162     default: 
00163         break ;
00164     }
00165     return( next ) ;
00166 }
00167 
00168 /** Notice of the goal
00169  */
00170 void showGoal(void)
00171 {
00172     int x0, y0, x1, y1 ;
00173     x0 = ((MAZE_W/2)-4) * CELL_W + LEFT_OFFSET ;
00174     y0 = ((MAZE_H/2)-1) * CELL_H + TOP_OFFSET ;
00175     x1 = ((MAZE_W/2)+4) * CELL_W + LEFT_OFFSET ;
00176     y1 = ((MAZE_H/2)+1) * CELL_H + TOP_OFFSET ;
00177 
00178     TFT.BusEnable(true) ;
00179 //    TFT.set_font((unsigned char*) Arial12x12);
00180     TFT.set_font((unsigned char*) Terminal6x8) ;
00181     TFT.fillrect(x0, y0, x1, y1, Blue) ;
00182     TFT.rect(x0,y0,x1,y1, Yellow) ;
00183     TFT.foreground(Red) ;
00184     TFT.background(Blue) ;
00185     x0 = ((MAZE_W/2) - 2) * CELL_W + LEFT_OFFSET ;
00186     y0 = (MAZE_H/2) * CELL_H - 3 + TOP_OFFSET ;
00187     TFT.locate(x0, y0) ;
00188     TFT.printf("G O A L") ;
00189     TFT.foreground(Yellow) ;
00190     TFT.background(Black) ;
00191     TFT.locate(30, 300) ;
00192 //    TFT.printf("Use FRDM touch slider") ;
00193     TFT.printf("           Retry!      ") ;
00194     TFT.locate(5, 300) ;
00195     TFT.printf("<< Prev") ;
00196     TFT.locate(180, 300) ;
00197     TFT.printf("Next >>") ;
00198     TFT.BusEnable(false) ;
00199 }
00200 
00201 /** Check if we can move to the next position
00202  * @param pos_type next the position we'd like to move next
00203  * @returns if the position is empty (movable)
00204  */
00205 bool checkMove(pos_type next) 
00206 {
00207     bool result = false ;
00208     
00209     switch(maze[next.y][next.x]) {
00210     case POS_PATH:
00211     case POS_GOAL:
00212         result = true ;
00213         break ;
00214     case POS_START:
00215     case POS_WALL:
00216     default:
00217         result = false ;
00218         break ;
00219     } 
00220     return( result ) ;
00221 }
00222 
00223 /** main a simple maze program
00224  */
00225 void doMaze(void) 
00226 {
00227     float res[3] ;
00228     pos_type current, next, goal ;
00229     int direction = DIR_STAY ;
00230     int x, y ;
00231     
00232     drawTFTMaze(&current, &goal) ;
00233     
00234     for (;;) {
00235         acc->getAccAllAxis(res) ;
00236         direction = getDirection(res) ;
00237         next = getNext(current, direction) ;
00238         if ((!isSame(current, next)) && checkMove(next)) {
00239            TFT.BusEnable(true) ;
00240            x = current.x * CELL_W + LEFT_OFFSET ;
00241            y = current.y * CELL_H + TOP_OFFSET ;
00242            TFT.fillrect(x, y, x+CELL_W-1, y+CELL_H-1, Black) ;
00243            x = next.x * CELL_W + LEFT_OFFSET ;
00244            y = next.y * CELL_H + TOP_OFFSET ;
00245            TFT.fillcircle(x+CELL_S, y+CELL_S, CELL_R, Green) ; 
00246            TFT.BusEnable(false) ;
00247            current = next ;
00248            if (isSame(next, goal)) {
00249              break ; // goal in!
00250            }
00251         }
00252         wait(0.1) ;
00253     }
00254     showGoal() ;
00255 }