maze.cpp

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