/* Thomas Braunl/Birgit Graf, UWA, 1998 */
/* Thomas Braunl, UWA, 2000 */
#include "eyebot.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#ifndef RIGHT
#define RIGHT 1
#endif
#ifndef LEFT
#define LEFT -1
#endif
#define MIDDLE 0
#define THRES 240 /* open way to next field? */
/* #define THRES_WALL 95 */
#define THRES_WALL 120 /* stop in front of wall */
#define PSDSIDE 100 /* PSD-value on side if robot in middle of corridor [mm] */
#define MAZESIZE 8
/* Define the places output can go */
#define LCD 0
#define CONSOLE 1
/* pos.0,0 is bottom left, dir. 0 is facing up (north) */
int mark[MAZESIZE][MAZESIZE]; /* 1 if visited */
int wall[MAZESIZE+1][MAZESIZE+1][2]; /* 1 if wall, 0 if free, -1 if unknown */
/* BOTTOM: wall[x][y][0] LEFT: wall[x][y][1] */
int map [MAZESIZE][MAZESIZE]; /* distance to goal */
int nmap[MAZESIZE][MAZESIZE]; /* copy */
int path[MAZESIZE*MAZESIZE]; /* shortest path */
/** handles for PSD sensors */
PSDHandle psd_front,psd_left,psd_right;
/** handle for motors */
VWHandle vw;
int GRAPH,DEBUG,DEBUG2;
/** robo position and orientation.
dir = 0, 1, 2, 3 equals: north, west, south, east.
*/
int rob_x, rob_y, rob_dir;
/** flag, set to end driving */
int end_drive=FALSE;
meterPerSec lin_speed=0.2; /* linear speed [m/s] */
radPerSec rot_speed=1.0; /* rotational speed [rad/s] */
float dist; /* length of one field (depends on lin. speed) */
/** where output should go */
int output;
/** play tune.
Play tune when reached target
*/
void tune()
{
AUTone(1046.5, 200);
while(!AUCheckTone()){}
AUTone(1396.9, 200);
while(!AUCheckTone()){}
AUTone(1046.5, 200);
while(!AUCheckTone()){}
AUTone(698.5, 200);
while(!AUCheckTone()){}
}
void set_speed()
{
float boc[2];
int ind = 0;
int end_proc=FALSE;
LCDClear();
LCDMenu(" + "," - ","Nxt","OK");
LCDPrintf("Set speed:\n");
boc[0]=lin_speed;
boc[1]=rot_speed;
while (!end_proc)
{
LCDSetPos(2,0);
LCDPrintf("lin. spd.: %1.2f\n",lin_speed);
LCDPrintf("rot. spd.: %1.2f\n",rot_speed);
LCDSetChar(2+ind,15,'*');
switch (KEYGet())
{
case KEY1:
boc[ind]+=0.02;
break;
case KEY2:
boc[ind]-=0.02;
break;
case KEY3:
LCDSetChar(2+ind,15,' ');
ind++;
if (ind>1) ind=0;
break;
case KEY4:
LCDClear(); end_proc=TRUE; break;
default: break;
}
lin_speed=boc[0];
rot_speed=boc[1];
}
}
/** print mark on LCD (6 lines).
print positions that robot has already visited.
*/
void print_mark()
{ int i,j;
if( output == LCD )
{
LCDSetPos(1,0);
for (i=5; i>=0; i--)
{ for (j=0; j<14; j++)
if (mark[i][j]) LCDPutChar('x'); else LCDPutChar('.');
if (i>0) LCDPutChar('\n');
}
}
else
{
fprintf(stderr,"MARK\n");
for (i=MAZESIZE-1; i>=0; i--)
{ for (j=0; j<MAZESIZE; j++)
if (mark[i][j]) fprintf(stderr,"x "); else fprintf(stderr,". ");
fprintf(stderr,"\n");
}
fprintf(stderr,"\n");
}
}
/** print full maze
print maze as explored by robot.
*/
void print_maze()
{ int i,j;
if( output == LCD )
{
LCDSetPos(1,0);
for (i=5; i>=0; i--)
{ for (j=0; j<=6; j++)
{ if (wall[i][j][1]==1) LCDPutChar('|'); /* left */
else if (wall[i][j][1]==0) LCDPutChar(' '); else LCDPutChar('.');
if (wall[i][j][0]==1) LCDPutChar('_'); /* bottom */
else if (wall[i][j][0]==0) LCDPutChar(' '); else LCDPutChar('.');
}
if (i>0) LCDPutChar('\n');
}
}
else
{
fprintf(stderr,"MAZE\n");
for (i=MAZESIZE; i>=0; i--)
{ for (j=0; j<=MAZESIZE; j++)
{ if (wall[i][j][1]==1) fprintf(stderr,"|"); /* left */
else if (wall[i][j][1]==0) fprintf(stderr," "); else fprintf(stderr,".");
if (wall[i][j][0]==1) fprintf(stderr,"_"); /* top */
else if (wall[i][j][0]==0) fprintf(stderr," "); else fprintf(stderr,".");
}
fprintf(stderr,"\n");
}
fprintf(stderr,"\n");
}
}
void wall_set(int *w, int v)
{ if (*w == -1) *w = v; /* not seen before, set value */
else if (*w != v) /* seen before and CONTRADITION */
{ *w = 1; /* assume wall to be safe */
LCDSetString(0,0,"CONTRADICTION\n\a");
}
}
/** maze_entry.
enter recognized walls or doors.
*/
void maze_entry(int x, int y, int dir, int open)
{ switch(dir) /* record bottom or left wall per square */
{ case 0: wall_set(&wall[y+1][x ][0], !open); break; /* top = bottom of next */
case 2: wall_set(&wall[y ][x ][0], !open); break;
case 1: wall_set(&wall[y ][x ][1], !open); break;
case 3: wall_set(&wall[y ][x+1][1], !open); break; /* right = left of next */
}
}
/** init_maze.
inits internal map of maze.
set marks to 0 and walls to -1 (unknown).
*/
void init_maze()
{ int i,j;
for (i=0; i<MAZESIZE; i++) for (j=0; j<MAZESIZE; j++)
mark[i][j] = 0;
for (i=0; i<MAZESIZE+1; i++) for (j=0; j<MAZESIZE+1; j++)
{ wall[i][j][0] = -1; wall[i][j][1] = -1; }
}
int xneighbor(int x, int dir)
{ switch (dir)
{ case 0: return x; /* north */
case 1: return x-1; /* west */
case 2: return x; /* south */
case 3: default: return x+1; /* east */
}
}
int yneighbor(int y, int dir)
{ switch (dir)
{ case 0: return y+1; /* north */
case 1: return y; /* west */
case 2: return y-1; /* south */
case 3: default: return y; /* east */
}
}
int unmarked(int y, int x, int dir)
{ dir = (dir+4) % 4;
return !mark [yneighbor(y,dir)] [xneighbor(x,dir)];
}
/** sign.
Return -1 for input<0, 1 for input>0 and 0 for input=0.
*/
int sign(float number)
{
if (number!=0.0)
return (int) number/fabs(number);
else
return 0;
}
/** Read PSD.
Read PSD value for specified sensor 3 times and return median
*/
int ReadPSD(int side)
{
int i,val[3];
switch (side)
{
case RIGHT:
for (i=0;i<3;i++)
val[i]=PSDGet(psd_right);
break;
case LEFT:
for (i=0;i<3;i++)
val[i]=PSDGet(psd_left);
break;
case MIDDLE:
for (i=0;i<3;i++)
val[i]=PSDGet(psd_front);
break;
}
if (val[0]<val[1])
if (val[1]<val[2])
return val[1];
else
if (val[0]<val[2])
return val[2];
else
return val[0];
else
if (val[1]<val[2])
if (val[0]<val[2])
return val[2];
else
return val[0];
else
return val[1];
}
/** go_to.
walk one square in current direction - drive curve if robot isn't in the middle
of the field or heading isn't straight.
*/
void go_to(int dir)
{
int turn;
int old_l,old_r,act_l,act_r;
float old_pos,pos_diff;
PositionType pos;
int epsilon=50;
float c=0.0015;
dir = (dir+4) % 4; /* keep goal dir in 0..3 */
turn = dir - rob_dir;
if (turn == 3) turn = -1; /* turn shorter angle */
else if (turn == -3) turn = 1;
if (turn && !end_drive)
{
VWDriveTurn(vw, turn*M_PI/2.0-sign(turn)*rot_speed/10, rot_speed); /* turn */
while (!VWDriveDone(vw)&&!end_drive)
end_drive=(KEYRead()==KEY3);
VWSetSpeed(vw,0,0);
}
if(!end_drive)
{
VWGetPosition(vw,&pos);
if (dir%2) /* heading west or east */
old_pos=pos.y;
else /* heading north or south */
old_pos=pos.x;
act_r=ReadPSD(RIGHT);
act_l=ReadPSD(LEFT);
do
{
old_r=act_r;
old_l=act_l;
act_r=ReadPSD(RIGHT);
act_l=ReadPSD(LEFT);
if (abs(act_r+act_l-old_r-old_l)<epsilon)
{
if(act_l<2*PSDSIDE&&act_r<2*PSDSIDE) /* walls on both sides */
VWSetSpeed(vw,lin_speed,c*(act_l-act_r));
else
if (act_l<2*PSDSIDE&&act_r>2*PSDSIDE) /* wall on left side */
VWSetSpeed(vw,lin_speed,2*c*(act_l-PSDSIDE));
else
if (act_l>2*PSDSIDE&&act_r<2*PSDSIDE) /* wall on right side */
VWSetSpeed(vw,lin_speed,2*c*(PSDSIDE-act_r));
else /* no walls */
VWSetSpeed (vw,lin_speed,0);
}
end_drive=(KEYRead()==KEY3);
VWGetPosition(vw,&pos);
if (dir%2) /* heading west or east */
pos_diff=fabs(old_pos-pos.y);
else /* heading north or south */
pos_diff=fabs(old_pos-pos.x);
} while ((pos_diff<dist)&&!end_drive&&!(ReadPSD(MIDDLE)<THRES_WALL));
rob_dir = dir;
rob_x = xneighbor(rob_x,rob_dir);
rob_y = yneighbor(rob_y,rob_dir);
LCDSetPos(0,0);
LCDPrintf("Pos[%2d,%2d,%1d]", rob_x,rob_y,rob_dir);
LCDSetPos(1,0);
if (DEBUG)
VWSetSpeed(vw,0,0);
}
}
/** check_mark.
if ALL walls of a square are known, mark square as visited.
this avoids unnecessary exploration.
*/
void check_mark()
{ int i,j;
for (i=1; i<MAZESIZE; i++) for (j=0; j<MAZESIZE-1; j++)
/* careful: watch boundaries!! i from 1 / j until size-1 */
{ if (wall[i ][j][0] != -1 && wall[i][j ][1] != -1 && /* bottom / left */
wall[i+1][j][0] != -1 && wall[i][j+1][1] != -1) /* top / right */
mark[i][j] = 1;
}
}
/** explore.
search maze goal from given start position and orientation.
if more than one possible way: search all recursively.
mark all visited maze squares.
*/
void explore()
{
int front_open, left_open, right_open, old_dir,key;
if(!end_drive)
{
end_drive=(KEYRead()==KEY3);
mark[rob_y][rob_x] = 1; /* mark current square */
front_open = ReadPSD(MIDDLE) > THRES;
left_open = ReadPSD(LEFT) > THRES;
right_open = ReadPSD(RIGHT) > THRES;
maze_entry(rob_x,rob_y,rob_dir, front_open);
maze_entry(rob_x,rob_y,(rob_dir+1)%4, left_open);
maze_entry(rob_x,rob_y,(rob_dir+3)%4, right_open);
check_mark();
old_dir = rob_dir;
if (GRAPH)
{ LCDSetPos(0,0);
LCDPrintf("Pos[%2d,%2d,%1d]", rob_x,rob_y,rob_dir);
if (left_open) LCDSetChar(0,13,'<');
else LCDSetChar(0,13,'|');
if (front_open) LCDSetChar(0,14,'^');
else LCDSetChar(0,14,'-');
if (right_open) LCDSetChar(0,15,'>');
else LCDSetChar(0,15,'|');
print_maze();
}
if (DEBUG)
{ LCDMenu("Next"," ","STOP"," ");
key=KEYGet();
end_drive=(key==KEY3);
}
if (!end_drive)
{ LCDMenu(" "," ","STOP"," ");
if (front_open && unmarked(rob_y,rob_x,old_dir)) /* then go straight */
{ go_to(old_dir); /* go 1 forward, 0 if first choice */
explore(); /* recursive call */
go_to(old_dir+2); /* go 1 back */
}
if (left_open && unmarked(rob_y,rob_x,old_dir+1)) /* then turn left */
{ go_to(old_dir+1); /* go 1 left */
explore(); /* recursive call */
go_to(old_dir-1); /* go 1 right, -1 = +3 */
}
if (right_open && unmarked(rob_y,rob_x,old_dir-1)) /* then turn right */
{ go_to(old_dir-1); /* go 1 right, -1 = +3 */
explore(); /* recursive call */
go_to(old_dir+1); /* go 1 left */
}
}
}
else
VWSetSpeed(vw,0,0);
}
/** print shortest distances from start on LCD (6 lines).
*/
void print_map()
{ int i,j;
if( output == LCD )
{
LCDClear();
LCDPutString("Map distances\n");
for (i=5; i>=0; i--)
{ for (j=0; j<4; j++)
LCDPrintf("%3d",map[i][j]);
if (i>0) LCDPutChar('\n');
}
}
else
{
fprintf(stderr,"MAP\n");
for (i=MAZESIZE-1; i>=0; i--)
{ for (j=0; j<MAZESIZE; j++)
fprintf(stderr,"%3d",map[i][j]);
fprintf(stderr,"\n");
}
fprintf(stderr,"\n");
}
}
/** shortest_path.
analyze shortest path after maze has been searched.
returns path length to goal.
or -1 if no path could be found.
*/
int shortest_path(int goal_y, int goal_x)
{ int i,j,iter;
LCDSetString(0,0," "); /* clear top line */
LCDSetPos(0,0); LCDPutString("Map..");
for (i=0; i<MAZESIZE; i++) for (j=0; j<MAZESIZE; j++)
{ map [i][j] = -1; /* init */
nmap[i][j] = -1;
}
map [0][0] = 0;
nmap[0][0] = 0;
iter=0;
do
{ iter++;
for (i=0; i<MAZESIZE; i++) for (j=0; j<MAZESIZE; j++)
{ if (map[i][j] == -1)
{ if (i>0)
if (!wall[i][j][0] && map[i-1][j] != -1)
nmap[i][j] = map[i-1][j] + 1;
if (i<MAZESIZE-1)
if (!wall[i+1][j][0] && map[i+1][j] != -1)
nmap[i][j] = map[i+1][j] + 1;
if (j>0)
if (!wall[i][j][1] && map[i][j-1] != -1)
nmap[i][j] = map[i][j-1] + 1;
if (j<MAZESIZE-1)
if (!wall[i][j+1][1] && map[i][j+1] != -1)
nmap[i][j] = map[i][j+1] + 1;
}
}
for (i=0; i<MAZESIZE; i++) for (j=0; j<MAZESIZE; j++)
map[i][j] = nmap[i][j]; /* copy back */
if (DEBUG2)
{ print_map();
LCDMenu("Next"," "," "," ");
KEYWait(KEY1);
LCDMenu(" "," "," "," ");
}
} while (map[goal_y][goal_x] == -1 && iter < (MAZESIZE*MAZESIZE) );
return map[goal_y][goal_x];
}
/** build path.
build shortest path after finding it.
uses map and wall.
sets path.
*/
void build_path(int i, int j, int len)
{ int k;
LCDSetString(0,0," "); /* clear top line */
LCDSetString(0,0,"Path..");
if (i<=5 && j<=6) LCDSetChar(6-i,2*j+1,'G'); /* mark goal */
for (k = len-1; k>=0; k--)
{
if (i>0 && !wall[i][j][0] && map[i-1][j] == k)
{ i--;
path[k] = 0; /* north */
}
else
if (i<MAZESIZE-1 && !wall[i+1][j][0] && map[i+1][j] == k)
{ i++;
path[k] = 2; /* south */
}
else
if (j>0 && !wall[i][j][1] && map[i][j-1] == k)
{ j--;
path[k] = 3; /* east */
}
else
if (j<MAZESIZE-1 && !wall[i][j+1][1] && map[i][j+1] == k)
{ j++;
path[k] = 1; /* west */
}
else
{ LCDPutString("ERROR\a");
KEYWait(ANYKEY);
}
/* mark path in maze on LCD */
if (i<=5 && j<=6) { if (k>0) LCDSetChar(6-i,2*j+1,'*'); /* path */
else LCDSetChar(6-i,2*j+1,'S'); /* start */
}
if (DEBUG2) fprintf(stderr,"path %3d:%1d\n", k, path[k]);
}
LCDSetString(0,6,"done");
}
/** drive path.
drive path after building it.
parametere specifies start to finish (0).
or finish to start (1).
*/
void drive_path(int len, int reverse)
{ int i;
if (reverse)
{ for (i=len-1; i>=0; i--) go_to(path[i]+2);
if (rob_dir != 0) /* back in start field */
{ VWDriveTurn(vw, -rob_dir*M_PI/2.0, rot_speed); /* turn */
VWDriveWait(vw);
rob_dir = 0;
}
}
else
for (i=0; i<len; i++) go_to(path[i]);
}
/** main program.
search maze from start in (0,0).
search whole maze and generate map.
@AUTHOR Thomas Braunl/Birgit Graf, UWA, 1998.
*/
int main ()
{ int key,path_len,trash,i;
int goalY, goalX, incr;
/* ask where to send output to */
LCDPrintf( "Format output\nfor?" );
LCDMenu( "LCD", "", "", "SIM" );
do { key = KEYGet(); }
while( key != KEY1 && key != KEY4 );
if( key == KEY1 ) output = LCD;
else output = CONSOLE;
LCDClear();
LCDPrintf("MAZE\nfull search\n\n");
init_maze();
/* init PSD sensors and motors */
psd_front = PSDInit(PSD_FRONT);
psd_left = PSDInit(PSD_LEFT);
psd_right = PSDInit(PSD_RIGHT);
PSDStart(psd_front|psd_left|psd_right,TRUE);
for (i=0;i<5;i++) /* get bad initial PSD-values and throw them away */
{
trash=ReadPSD(RIGHT);
trash=ReadPSD(LEFT);
trash=ReadPSD(MIDDLE);
}
vw = VWInit(VW_DRIVE,1);
VWStartControl(vw,7,0.3,7,0.1);
set_speed();
dist=0.375-lin_speed/10; /* distance from one field to next [m] */
LCDPrintf("Put robot to\n(0,0,north)\n");
end_drive=FALSE;
LCDMenu("GO","Fst","Dbg","Db2");
key = KEYGet();
GRAPH = key != KEY2;
DEBUG = key == KEY3;
DEBUG2 = (key == KEY3) || (key == KEY4);
LCDMenu(" "," ","STOP"," ");
rob_x = 0; rob_y=0; rob_dir = 0; /* start in pos. 0,0, facing north */
explore();
/* back in [0,0] turn robot to original direction */
if (rob_dir != 0)
{ VWDriveTurn(vw, -rob_dir*M_PI/2.0, rot_speed); /* turn */
VWDriveWait(vw);
rob_dir = 0;
}
VWSetSpeed(vw,0,0);
end_drive=FALSE;
do
{ print_maze();
LCDMenu("Y+-","X+-","+/-","GO");
incr = 1;
goalX=0; goalY=0;
LCDMode(SCROLLING|CURSOR);
do
{ LCDSetPos(0,0);
LCDPrintf("GOAL y,x: %2d %2d\a", goalY, goalX);
if (goalY<=5 && goalX <=6) LCDSetPos(6-goalY,2*goalX+1);
switch (key = KEYGet())
{ case KEY1: goalY = (goalY+incr+MAZESIZE) % MAZESIZE; break;
case KEY2: goalX = (goalX+incr+MAZESIZE) % MAZESIZE; break;
case KEY3: incr = -incr; break;
}
} while (key != KEY4);
LCDMode(SCROLLING|NOCURSOR);
LCDMenu(" "," "," "," ");
path_len = shortest_path(goalY,goalX); /* from start 0,0 to goal */
if (path_len != -1) /* if path exists */
{
LCDSetPos(0,0); LCDPrintf("Path length: %3d", path_len);
build_path(goalY,goalX,path_len);
do /* optional data display */
{ LCDMenu("Map","Mrk","Maz","DRV");
switch (key = KEYGet())
{ case KEY1: print_map() ; break;
case KEY2: print_mark(); break;
case KEY3: print_maze(); break;
}
} while (key != KEY4);
if(!end_drive)
{ LCDMenu(" "," ","STOP"," ");
drive_path(path_len,0); /* drive start to finish */
if(!end_drive)
{ VWSetSpeed(vw,0,0);
LCDPrintf("reached target\n\n");
tune();
drive_path(path_len,1); /* drive finish to start */
if(!end_drive)
{ VWSetSpeed(vw,0,0);
LCDPrintf("reached start\n");
tune(); tune();
}
}
}
}
else { LCDSetPos(0,0); LCDPrintf("No path exists! \a"); }
end_drive=FALSE;
LCDMenu("REP"," "," ","END");
} while (KEYGet()!=KEY4);
VWRelease(vw);
PSDRelease();
return 0;
}