main.cpp

00001 /** maze_vt100 a simple maze using vt100 and MMA8451Q libraries
00002  */
00003 #include "mbed.h"
00004 #include "MMA8451Q.h"
00005 #include "vt100.h"
00006 #include "maze.h"
00007 
00008 #define DIR_STAY  0
00009 #define DIR_UP    1
00010 #define DIR_DOWN  2
00011 #define DIR_RIGHT 3 
00012 #define DIR_LEFT  4
00013 
00014 #define MMA8451_I2C_ADDRESS (0x1D<<1)
00015 
00016 typedef struct _pos {
00017     int x ;
00018     int y ;
00019 } pos_type ;
00020 
00021 MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS) ;
00022 vt100 tty ;
00023 float threshold = 0.2 ;
00024 
00025 /** Check if two pos_type values are same
00026  * @param pos_type a
00027  * @param pos_type b
00028  * @returns if a and b are same position
00029  */
00030 bool isSame(pos_type a, pos_type b)
00031 {
00032     return((a.x == b.x)&&(a.y == b.y)) ;
00033 }
00034 
00035 /** Draw the maze defined in the "maze.h"
00036  * @param pos_type *current actually the start point
00037  * @param pos_type *goal the position of the goal
00038  * @note those params are actually returned by this function
00039  */
00040 void drawTextMaze(pos_type *current, pos_type *goal) 
00041 {
00042     int x, y ;
00043     char c ;
00044     
00045     tty.setBG(7) ; // set background to white
00046     tty.setFG(0) ; // set foreground to black
00047     tty.cls() ;
00048     
00049     for (y = 0 ; y < MAZE_H ; y++ ) {
00050         for (x = 0 ; x < MAZE_W ; x++ ) {
00051             switch(maze[y][x]) {
00052             case 0:  // path
00053                 c = ' ' ; 
00054                 break ; 
00055             case 1:  // wall
00056                 c = ' ' ; 
00057                 tty.setBG(0) ; // set background to black
00058                 tty.setFG(7) ; // set foreground to white
00059                 break ; 
00060             case 2: // Start point  
00061                 c = 'S' ;
00062                 current->x = x ;
00063                 current->y = y ; 
00064                 break ; // start
00065             case 3: // Goal  
00066                 c = 'G' ;
00067                 tty.green() ;
00068                 goal->x = x ;
00069                 goal->y = y ; 
00070                 break ; 
00071             default: // should not be here 
00072                 c = '?' ; 
00073                 break ; // wth?
00074             }
00075             tty.putChar(x+1, y+1, c) ;
00076             tty.setBG(7) ; // set background to white
00077             tty.setFG(0) ; // set foreground to black
00078         }
00079         printf("\n\r") ;
00080     }
00081 }
00082 
00083 /** Filter out too little move 
00084  * @param float in returned value from the acc
00085  * @returns float result filtered value of in
00086  */
00087 float filterVal(float in)
00088 {
00089     float result = 0.0 ;
00090     if ((-threshold > in)||(in > threshold)) {
00091         result = in ;
00092     }
00093     return( result ) ;
00094 }
00095 
00096 /** Decide which direction to go
00097  * @param float res[] acc value of x, y 
00098  * @returns int direction to move 
00099  */
00100 int getDirection(float res[])
00101 {
00102     float dx, dy ;
00103     int direction = DIR_STAY ;
00104     dx = filterVal(res[0]) ;
00105     dy = filterVal(res[1]) ;
00106 
00107     if ((dx*dx) > (dy*dy)) { // holizontal move
00108         if (dx > 0.0) {
00109             direction = DIR_DOWN ;
00110         } else if (dx < 0.0) {
00111             direction = DIR_UP ;
00112         }
00113     } else { // vertical move
00114         if (dy > 0.0) {
00115             direction = DIR_RIGHT ;
00116         } else if (dy < 0.0) {
00117             direction = DIR_LEFT ;
00118         }
00119     }
00120     return(direction) ;
00121 }
00122 
00123 /** Get next positon to move to
00124  * @param pos_type current where we are now
00125  * @param int direction which way we'd like to move
00126  * @returns the candidate positon for the next move
00127  */
00128 pos_type getNext(pos_type current, int direction) 
00129 {
00130     pos_type next = current ;
00131     switch(direction) {
00132     case DIR_STAY: 
00133         break ;
00134     case DIR_UP: 
00135         if (next.y > 0) { 
00136             next.y-- ; 
00137         }  
00138         break ;
00139     case DIR_DOWN: 
00140         if (next.y < (MAZE_H - 1)) { 
00141             next.y++ ; 
00142         } 
00143         break ;
00144     case DIR_RIGHT: 
00145         if (next.x < (MAZE_W - 1)) { 
00146             next.x++ ; 
00147         } 
00148         break ;
00149     case DIR_LEFT: 
00150         if (next.x > 0) { 
00151             next.x-- ; 
00152         } 
00153         break ;
00154     default: 
00155         break ;
00156     }
00157     return( next ) ;
00158 }
00159 
00160 /** Notice of the goal
00161  */
00162 void showGoal(void)
00163 {
00164     tty.blue() ;
00165     tty.frame((MAZE_W/2)-4,(MAZE_H/2)-1,(MAZE_W/2)+4,(MAZE_H/2)+1) ;
00166     tty.red() ;
00167     tty.locate((MAZE_W/2)-3, (MAZE_H/2)) ;
00168     printf("G O A L\n\r") ;
00169 }
00170 
00171 /** Check if we can move to the next position
00172  * @param pos_type next the position we'd like to move next
00173  * @returns if the position is empty (movable)
00174  */
00175 bool checkMove(pos_type next) 
00176 {
00177     bool result = false ;
00178     
00179     switch(maze[next.y][next.x]) {
00180     case POS_PATH:
00181     case POS_GOAL:
00182         result = true ;
00183         break ;
00184     case POS_START:
00185     case POS_WALL:
00186     default:
00187         result = false ;
00188         break ;
00189     } 
00190     return( result ) ;
00191 }
00192 
00193 /** main a simple maze program
00194  */
00195 int main() {
00196     float res[3] ;
00197     pos_type current, next, goal ;
00198     int direction = DIR_STAY ;
00199     
00200     tty.cls() ;
00201     drawTextMaze(&current, &goal) ;
00202     tty.black() ;
00203     tty.putChar(current.x+1, current.y+1, '@') ;
00204     printf("\r\n") ;
00205     
00206     for (;;) {
00207         acc.getAccAllAxis(res) ;
00208         direction = getDirection(res) ;
00209         next = getNext(current, direction) ;
00210         if ((!isSame(current, next)) && checkMove(next)) {
00211            tty.putChar(current.x+1, current.y+1, ' ') ;
00212            tty.putChar(next.x+1, next.y+1, '@') ;
00213            tty.green() ;
00214            tty.putStr(1, MAZE_H+1, "\r\n") ;
00215            tty.black() ;
00216            current = next ;
00217            if (isSame(next, goal)) {
00218              break ; // goal in!
00219            }
00220         }
00221         wait(0.1) ;
00222     }
00223     showGoal() ;
00224     for (;;) {
00225         // wait for ever for reset
00226     }
00227 }