/*--------------------------------------------------------------------------
| Filename: map-socbot.c (based on map-eve.c;
| map-eve.c is derived from drive.c)
|
| Author: Nicholas Tay, UWA 1998
| Changes: Thomas Braunl, UWA 2000
| T. Y. Ng, UWA Dec, 2001
| Modified: W. Chen, UWA Feb, 2002
|
| Description: Map Generation for the Eyebot embedded in soccer robot.
| Note: This program currently only works for soccer robot #2
|
----------------------------------------------------------------------- */
#include "eyebot.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
typedef struct {
int x1;
int y1;
int x2;
int y2;
} Segment; /* Representation of XSegments - stores line-segments */
#define GRID_WIDTH 80 /* The width of each cell */
#define GRID_DIMENSIONS 80 /* The dimension of the Grid */
#define GRID_CENTER 40 /* Half the GRID_DIMENSIONS */
#define PSD_MAX 210 /* Maximum PSD reliable range including OFFSET */
#define PSD_LIMIT 400
#define OFFSET_RIGHT 0 /* (original=38) Offset value for Right PSD */
#define OFFSET_LEFT 0 /* (original=38) Offset value for Left PSD */
#define OFFSET_FRONT 0 /* (original=0) Offset value for Front PSD */
#ifndef ROBOT_RADIUS
#define ROBOT_RADIUS 80 /* (original=75) Chosen radius for soccer
robot that is rectanglar in shape */
#endif
#define CLEAR 0 /* Sets a cell to "clear" */
#define BLOCKED 1 /* Sets a cell to "blocked" */
#define NOT_BORDER 3 /* Sets a cell to "not map border" */
#define BLOCKED_UNEXP 4 /* Sets a cell to "blocked but unexplored" */
#define MAP_BORDER 5 /* Sets a cell to "map border" */
#define CLEAR_UNEXP 6 /* Sets a cell to "clear but unexplored" */
#define V_LIN 7.0
#define T_LIN 0.3
#define V_ROT 10.0
#define T_ROT 0.1
#define IS_EXPLORED -1
#define NOT_CLOSE 0
#define IS_CLOSE 1
#define NUM_PSD 3
#define LEFT_MIN 60 /* 60mm is the min working distance of left psd */
#define FRONT_MIN 70 /* 70mm is the min working distance of front psd */
#define RIGHT_MIN 60 /* 60mm is the min working distance of right psd */
#define FRONT_GAP 90
#define FOLLOW_GAP 85 /* for left or right boundary following routine */
#define ADJUST_LIMIT 100
#define LEFT_PSD 0
#define FRONT_PSD 1
#define RIGHT_PSD 2
/* new-1:--------------------= front_too_close =-------------------- */
/** Checks if front of robot is too close to an obstacle using front psd.
@param vw_ The VW handle
@param front_psd The front psd sensor
@return TRUE if it is too close
FALSE otherwise */
int front_too_close(VWHandle vw_, PSDHandle front_psd)
{
int ref, front, value = FALSE;
ref = PSDGet(front_psd) - OFFSET_FRONT;
VWSetSpeed(vw_, -0.10, 0.0);/* move backwards; -0.10 chosen arbitarily */
OSWait(1/*20*/); /* wait for 20/100 seconds, 20 chosen arbitarily */
VWSetSpeed(vw_, 0.0, 0.0); /* stop robot */
front = PSDGet(front_psd) - OFFSET_FRONT;
if(front < ref || front == FRONT_MIN) value = TRUE; /* too close */
/*LCDPutString("front_too_close\n"); */
return value;
}
/* new-2:---------------------= stop_forward =---------------------- */
/** Stops the robot moving forwards.
@param vw_ The VW handle */
void stop_forward(VWHandle vw_)
{
VWSetSpeed(vw_, -0.2, 0.0);/* -0.2 chosen arbitarily */
VWSetSpeed(vw_, 0.0, 0.0);
}
/* new-3:---------------------= move_backward =---------------------- */
/** Moves the robot backwards using front psd.
@param vw_ The VW handle
@param front_psd The front psd sensor */
void move_backward(VWHandle vw_, PSDHandle front_psd)
{
int temp_dist;
do{
VWSetSpeed(vw_, -0.10, 0.0);/* move backwards; -0.10 chosen arbitarily */
temp_dist = PSDGet(front_psd) - OFFSET_FRONT;
/*LCDPutString("move_backward\n");*/
}while(temp_dist < 120);/* 120 chosen arbitarily */
VWSetSpeed(vw_, 0.0, 0.0);
}
/* 1--------------------------= VEHStop =--------------------------- */
/** Stops the robot.
@param vw_ The VW handle */
void VEHStop(VWHandle vw_)
{
VWSetSpeed(vw_, 0.0, 0.0);
}
/* 2--------------------------= MAP_Check =------------------------- */
/** Checks adjacent cells for a match in (value_).
@param map_ The map grid
@param pos_ The index of the grid
@param value_ The value of the cell that the routine tries to match
@return TRUE, If a match is found; FALSE otherwise
@see Display_MAP */
int MAP_Check(char *map_, int pos_, int value_)
{
int i, j, temp = FALSE;
for(i = -1; i < 2; i++)
for(j = -1; j < 2; j++)
/* Evaluates neighbouring cells to determine a match */
if(map_[pos_ - i * GRID_DIMENSIONS + j] == value_) temp = TRUE;
return temp;
}
/* 3-----------------------= MAP_Segment =-------------------------- */
/** Obtains the equivalent map position for the robot position.
@param vals_ The position of the robot
@return map position */
int MAP_Segment(float vals_)
{
return (int)(vals_ * 1000.0 + GRID_WIDTH * GRID_CENTER + 100);
}
/* 4-------------------------= MAP_Pos =---------------------------- */
/** Obtains the array index for a particular grid cell.
Given a particular robot position, returns the cell that
represents the robot position.
@param temp Current position of the robot
@param dist Distance received from the PSD sensor
@param offset_ The angle offset (Depends on which PSD used)
@param x The array index
@param y The array index
@see MAP_Locate MAP_Location */
void MAP_Pos(PositionType temp, int dist, float offset_, int *x, int *y)
{
float a, b;
a = temp.x + (float)dist*cos(temp.phi + offset_) / 1000.0;
b = temp.y + (float)dist*sin(temp.phi + offset_) / 1000.0;
*x = ((int)(a * 1000.0) / GRID_WIDTH) + GRID_CENTER;
*y = ((int)(-b * 1000.0) / GRID_WIDTH) + GRID_CENTER;
if(a < 0) *x -= 1;
if(b > 0) *y -= 1;
}
/* 5-----------------------= MAP_Angle =---------------------------- */
/** Finds the difference between two radians. The order of the radians
is important, as the function returns a positive difference.
@param x_ The first radians
@param y_ The second radians
@return The angle difference in radians. The angle is positive */
radians MAP_Angle(radians x_, radians y_)
{
if(x_ < 0) x_ += 2.0 * M_PI;
if(y_ < 0) y_ += 2.0 * M_PI;
if(y_ < x_) y_ += 2.0 * M_PI;
return (y_ - x_);
}
/* 6-------------------------= MAP_Location =----------------------- */
/** Obtains the location of a cell in the grid in terms of its robot position.
Performs the reverse function of "MAP_Pos".
The robot position returned is the center of the cell.
@param target The position (PositionType) of the cell
@param index_ The cell array index
@see MAP_Pos */
void MAP_Location(PositionType *target_, int index_)
{
int x, y;
y = index_ / GRID_DIMENSIONS;
x = index_ % GRID_DIMENSIONS;
target_->x = ((float)x - (float)GRID_CENTER + 0.5) * (float)GRID_WIDTH / 1000.0;
target_->y = ((float)y - (float)GRID_CENTER + 0.5) * (float)GRID_WIDTH / -1000.0;
}
/* 7----------------------= MAP_Position =--------------------------- */
/** VWGetPosition equivalent. Obtains the position of the robot.
However, this function updates the robot position to the grid map.
@param vw_ The VW handle for the robot
@param pos The position (PositionType) of the robot
@param map_ The grid for storage
@return The position of the robot */
int MAP_Position(VWHandle vw_, PositionType *pos_, char *map_)
{
int i = 0, j = 0, a, b;
VWGetPosition(vw_, pos_); /* Obtain robot position */
MAP_Pos(*pos_, 0, 0.0, &i, &j);
/* Updating the Grid Map */
if((map_[j * GRID_DIMENSIONS + i] != BLOCKED /*1*/) &&
(map_[j * GRID_DIMENSIONS + i] != MAP_BORDER /*5*/))
map_[j * GRID_DIMENSIONS + i] = CLEAR; /*0;*/
/* Updates its neighbouring cells to "clear" (0)
if they were initially defined as "clear but unexplored" (6) */
for(a = -1; a < 2; a++)
for (b = -1; b < 2; b++)
if(map_[(j + a) * GRID_DIMENSIONS + i + b] == CLEAR_UNEXP /*6*/)
map_[(j + a) * GRID_DIMENSIONS + i + b] = CLEAR; /*0;*/
return j * GRID_DIMENSIONS + i;
}
/* 8------------------------= MAP_Dist =---------------------------- */
/** Obtains the distance to the nearest obstacle using the right PSD sensor.
Records the obstacle and all spaces between the obstacle and
the robot into the grid. Also checks the left PSD sensor for obstacles.
@param vw_ The VW Handle
@param psd_ The right PSD sensor
@param psdLeft The left PSD sensor
@param map_ The grid for storage
@return The distance to the obstacle using the right PSD sensor */
int MAP_Dist (VWHandle vw_, PSDHandle psd_, PSDHandle psdLeft, char *map_)
{
int i, j, k, x = 0, y = 0, obstacle_, dist;
PositionType pos_n_;
/* Checks Left PSD Sensors */
dist = PSDGet(psdLeft) - OFFSET_LEFT;
VWGetPosition(vw_, &pos_n_);
if(dist < PSD_MAX){ /* An Obstacle is detected */
/* Marks the cell containing the obstacle as "blocked but unexplored"(4) */
MAP_Pos(pos_n_, dist + ROBOT_RADIUS, M_PI/2.0, &x, &y);
if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
if((map_[y * GRID_DIMENSIONS + x] != CLEAR/*0*/) && (map_[y * GRID_DIMENSIONS + x] != BLOCKED))
if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, BLOCKED /*1*/))
map_[y * GRID_DIMENSIONS + x] = BLOCKED_UNEXP; /*4;*/
/* Set the cell beside the robot as "clear"(0) */
if(dist > GRID_WIDTH /*80*/){
MAP_Pos(pos_n_, ROBOT_RADIUS, M_PI/2.0, &x, &y);
if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
map_[y * GRID_DIMENSIONS + x] = CLEAR; /*0;*/
}
}
else { /* Left Sensors did not detect an obstacle within out given range */
/* Sets the cell at the limit of PSD range to "clear but unexplored" (6) */
MAP_Pos(pos_n_, ROBOT_RADIUS + PSD_MAX, M_PI/2.0, &x, &y);
if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
if((map_[y * GRID_DIMENSIONS + x] != CLEAR) && (map_[y * GRID_DIMENSIONS + x] != BLOCKED))
if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, BLOCKED /*1*/))
if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, CLEAR /*0*/))
map_[y * GRID_DIMENSIONS + x] = CLEAR_UNEXP; /*6;*/
}
/* Checks Right PSD Sensor */
obstacle_ = PSDGet(psd_) - OFFSET_RIGHT;
VWGetPosition(vw_, &pos_n_);
if(obstacle_ < PSD_MAX){ /* An obstacle is detected */
dist = obstacle_;
for(k = 0; k < 5; k++){
MAP_Pos(pos_n_, dist + ROBOT_RADIUS, -M_PI/2.0, &x, &y);
if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS)){
/* The cell containing the obstacle is marked as "blocked" (1) */
if(dist == obstacle_){
if(map_[y * GRID_DIMENSIONS + x] != MAP_BORDER/*5*/)
map_[y * GRID_DIMENSIONS + x] = BLOCKED; /*1;*/
/* Neighbouring cells which are unexplored (4) or (6) are cleared */
for(i = -2; i < 3; i++)
for(j = -2; j < 3; j++)
if((((y + i) * GRID_DIMENSIONS + x + j) > 0) &&
((y + i) * GRID_DIMENSIONS + x + j) <
(GRID_DIMENSIONS * GRID_DIMENSIONS))
if((map_[(y + i) * GRID_DIMENSIONS + x + j] == BLOCKED_UNEXP/*4*/) ||
(map_[(y + i) * GRID_DIMENSIONS + x + j] == CLEAR_UNEXP/*6*/))
map_[(y + i) * GRID_DIMENSIONS + x + j] = NOT_BORDER;//3 is not cleared (0)
} /* Sets cells between the robot and obstacle as "clear" (0) */
else if((map_[y * GRID_DIMENSIONS + x] != BLOCKED/*1*/) &&
(map_[y * GRID_DIMENSIONS + x] != MAP_BORDER/*5*/))
map_[y * GRID_DIMENSIONS + x] = CLEAR; /*0;*/
}
dist -= obstacle_ / 4;
}
}
return obstacle_; /* Returns Right PSD Distance */
}
/* 9-----------------------= MAP_Orient =--------------------------- */
/** Obtains the orientation required for the robot to face the target position.
This routine is required for Path Navigation.
@param vw_ The VW handle
@param current The current position of the robot
@param pos_n The target destination for the robot
@return The orientation required for the robot (radians)
@see MAP_Navigation MAP_Target */
float MAP_Orient(VWHandle vw_, PositionType current, PositionType pos_n)
{
float angle = 0;
/* Using the robot's position, determines the desired orientation
* the robot must face to reach the destination */
if(current.x == pos_n.x){
if(pos_n.y >= current.y) return M_PI / 2.0;
else return -M_PI / 2.0;
}
angle = atan((pos_n.y - current.y) / fabs(pos_n.x - current.x));
if(pos_n.x < current.x) angle = M_PI - angle;
return angle;
}
/* 10---------------------= MAP_Navigation =------------------------ */
/** Performs a robot navigation routine to reach a destination.
Includes code for obstacle avoidance and boundary following.
Adapted from the DistBug architecture.
@param vw_ The VW handle
@param psd_x The PSD Handles for the left, front and right PSDs
@param target The target cell in the grid
@param map_ The grid map used for storage
@see Wall_Follow_Right MAP_Target MAP_Orient MAP_Location */
void MAP_Navigation (VWHandle vw_, PSDHandle * psd_x, int target, char *map_)
{
PositionType dest_n, pos_n_, stored;
SpeedType stopped_;
int i, j, dist_ = 0, prev_ = 0, x1, x2, y1, y2, looped = 0;
MAP_Location(&dest_n, target);
VWGetPosition(vw_, &stored);
LCDPutString("Navigate_target\n");
do{ /* Rotate robot until it faces the correct orientation to target */
VWSetSpeed(vw_, 0, 0.2); /* 0.2- magic number */
//LCDPutString("Left rotation\n");
VWGetPosition(vw_, &pos_n_);
}while(MAP_Angle(pos_n_.phi, MAP_Orient(vw_, pos_n_, dest_n)) > 0.03);/* 0.03- magic number */
VEHStop(vw_);
do{ /* Begins navigation trajectory */
VWSetSpeed(vw_, 0.13, 0); /* 0.13- magic number */
MAP_Position(vw_, &pos_n_, &map_[0]);
VWGetSpeed(vw_, &stopped_);
/* Condition to eliminate the target destination
* if the destination is unreachable (ie. no path exists ). */
if(looped != 0)
if((fabs(pos_n_.x - stored.x) <= 0.15) &&
(fabs(pos_n_.y - stored.y) <= 0.15)){ /* 0.15- magic number */
map_[target] = CLEAR; /*0;*/
return;
}
if((fabs(pos_n_.x - dest_n.x) + fabs(pos_n_.y - dest_n.y)) <
(fabs(stored.x - dest_n.x) + fabs(stored.y - dest_n.y))){
VWGetPosition(vw_, &stored);
looped = 0;
}
if((fabs(pos_n_.x - stored.x) > 0.15) && (fabs(pos_n_.y - stored.y) > 0.15))
looped = 1;
/* new-4: no obstacle in front or right of robot but its left is close to wall(s) */
if((PSDGet(psd_x[0]) - OFFSET_LEFT) < 100 &&
(PSDGet(psd_x[1]) - OFFSET_FRONT) > 100 &&
(PSDGet(psd_x[2]) - OFFSET_RIGHT) > 100 ){ // 100 chosen arbitarily
stop_forward(vw_);
VWSetSpeed(vw_, 0.0, -0.2); // rotate robot clockwise; -0.2 chosen arbitarily
while((PSDGet(psd_x[0]) - OFFSET_LEFT) < 90 /*100*/){
/*LCDPutString("L_close_Not_F/R\n");*/
}
VEHStop(vw_);
}
/* new-5: no obstacle in front or left of robot but its right is close to wall(s) */
if((PSDGet(psd_x[0]) - OFFSET_LEFT) > 100 &&
(PSDGet(psd_x[1]) - OFFSET_FRONT) > 100 &&
(PSDGet(psd_x[2]) - OFFSET_RIGHT) < 100 ){ // 100 chosen arbitarily
stop_forward(vw_);
VWSetSpeed(vw_, 0.0, 0.2); // rotate robot anti-clockwise; 0.2 chosen arbitarily
while((PSDGet(psd_x[2]) - OFFSET_RIGHT) < 90 /*100*/){
/*LCDPutString("R_close_Not_L/F\n");*/
}
VEHStop(vw_);
}
/* An obstacle is detected obstructing the robot's path in front of robot */
if(((PSDGet(psd_x[1]) - OFFSET_FRONT) < 100/*80*/) || ((stopped_.v < 0.05) && (stopped_.w == 0))){
/*LCDPutString("An obstacle\n"); */
stop_forward(vw_);/* stop moving forwards */
if((dist_=PSDGet(psd_x[0]) - OFFSET_LEFT) < (PSDGet(psd_x[2]) - OFFSET_RIGHT)){
/* The left side of the robot is closer to the obstacle,
* thus perform Boundary-Following using the left PSD Sensors */
VWSetSpeed(vw_, 0, -0.2);/* -0.2- magic number */
do{ /* Rotate the robot so that the robot is parallel to the left wall */
prev_ = dist_;
OSWait(1);
dist_ = PSDGet(psd_x[0]) - OFFSET_LEFT;
}while (prev_ >= dist_);
VEHStop(vw_);
MAP_Position(vw_, &pos_n_, &map_[0]);
/* Perform left boundary-following */
while((MAP_Angle(pos_n_.phi, MAP_Orient(vw_, pos_n_, dest_n)) > 0.05) &&
(MAP_Angle(MAP_Orient(vw_, pos_n_, dest_n),pos_n_.phi) > 0.05) &&
(fabs(pos_n_.x - dest_n.x) + fabs(pos_n_.y - dest_n.y) > 0.16)){/* 0.16- magic number */
dist_ = PSDGet(psd_x[0]) - OFFSET_LEFT;
MAP_Pos(pos_n_, ROBOT_RADIUS, 0, &x1, &y1);
MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, 0, &x2, &y2);
VWGetSpeed(vw_, &stopped_);
/* Condition for robot stalled (collision) */
if((stopped_.v < 0.05) && (stopped_.w == 0)){ /* 0.05- magic number */
stop_forward(vw_);
VWSetSpeed(vw_, 0, -0.30); /* -0.30- magic number */
}
/* Condition for boundary conditions (reach limits of grid map) */
else if((map_[y1 * GRID_DIMENSIONS + x1] == MAP_BORDER/*5*/) ||
(map_[y2 * GRID_DIMENSIONS + x2] == MAP_BORDER/*5*/)){
stop_forward(vw_);
VWSetSpeed(vw_, 0, -0.3);
}
/* Obstacle is detected in front of robot - rotates right to avoid */
else if((PSDGet(psd_x[1]) - OFFSET_FRONT) < 100/*80*/){/* 80 chosen arbitarily */
/*VEHStop(vw_);*/
stop_forward(vw_);
/* LCDPutString("L_elseif_fnt<100\n");*/
VWDriveTurn(vw_, -M_PI/4.0, 0.4); /* 0.4- magic number */
while(!VWDriveDone(vw_))
prev_ = PSDGet(psd_x[0]) - OFFSET_LEFT;
while((dist_ = PSDGet(psd_x[0]) - OFFSET_LEFT) <= prev_){
VWSetSpeed(vw_, 0, -0.30);
prev_ = dist_;
}
}
else{
MAP_Pos(pos_n_, ROBOT_RADIUS, 0, &x1, &y1);
MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, 0, &x2, &y2);
/* Condition for boundary limits to the left of robot -
* must proceed in the same direction */
if((map_[y1 * GRID_DIMENSIONS + x1] == MAP_BORDER /*5*/) ||
(map_[y2 * GRID_DIMENSIONS + x2] == MAP_BORDER /*5*/)){
VWSetSpeed(vw_, 0.15, 0); /* 0.15- magic number */
}
/* Possible collision with obstacle - collision avoidance routine */
else if((PSDGet(psd_x[0]) - OFFSET_LEFT) < 100 /*80*/){/* 80 chosen arbitarily */
stop_forward(vw_);
VWSetSpeed(vw_, 0, -0.2); /* -0.2- magic number */
/* LCDPutString("Left<100\n"); */
}
/* Robot lost the obstacle (outward corner).
* Curve left to maintain contact with the boundary */
else if((PSDGet(psd_x[0]) - OFFSET_LEFT) > 138){/* 38 + 100 = 138 */
VWSetSpeed(vw_, 0.05, 0.55); /* 0.05 & 0.55- magic numbers */
dist_ = prev_;
/* LCDPutString("else_curve_L\n"); */
}
/* Veering close to obstacle, adjust trajectory */
else if(dist_ > prev_){
VWSetSpeed(vw_, 0.15, 0.2);
/* LCDPutString("else_veer_close\n");*/
}
/* Veering away from obstacle, adjust trajectory */
else if(dist_ < prev_){
VWSetSpeed(vw_, 0.15, -0.2);
/* LCDPutString("else_veer_away\n"); */
}
/* Robot should maintain trajectory - moving parallel */
else{
VWSetSpeed(vw_, 0.15, 0);
/* LCDPutString("move parallel\n"); */
}
}
prev_ = dist_;
MAP_Position(vw_, &pos_n_, &map_[0]);
}
}
else{
/* The right side of the robot is closer to the wall -
* enact a Boundary-Following routine using the right sensors */
VWSetSpeed(vw_, 0, 0.2);
do{ /* Rotates robot so that robot is parallel to right wall */
prev_ = dist_;
OSWait(1);
dist_ = PSDGet(psd_x[2]) - OFFSET_RIGHT;
}while (prev_ >= dist_);
VEHStop(vw_);
MAP_Position(vw_, &pos_n_, &map_[0]);
/* Begin right wall-following routine */
while((MAP_Angle(pos_n_.phi, MAP_Orient(vw_, pos_n_, dest_n)) > 0.05) &&
(MAP_Angle(MAP_Orient(vw_, pos_n_, dest_n),pos_n_.phi) > 0.05) &&
(fabs(pos_n_.x - dest_n.x) + fabs(pos_n_.y - dest_n.y) > 0.16)){
dist_ = PSDGet(psd_x[2]) - OFFSET_RIGHT;
MAP_Pos(pos_n_, ROBOT_RADIUS, 0, &x1, &y1);
MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, 0, &x2, &y2);
VWGetSpeed(vw_, &stopped_);
/* Condition for robot stalled (collision) */
if((stopped_.v < 0.05) && (stopped_.w == 0)){
stop_forward(vw_);
VWSetSpeed(vw_, 0, 0.3);
}
/* Condition for reaching boundary limits */
else if((map_[y1 * GRID_DIMENSIONS + x1] == MAP_BORDER /*5*/) ||
(map_[y2 * GRID_DIMENSIONS + x2] == MAP_BORDER /*5*/)){
stop_forward(vw_);
VWSetSpeed(vw_, 0, 0.3);
}
/* Condition for obstacle in front of robot - rotate left to avoid */
else if((PSDGet(psd_x[1]) - OFFSET_FRONT) < 100/*80*/){/* 80 chosen arbitarily */
/*VEHStop(vw_);*/
stop_forward(vw_);
/*LCDPutString("R_elseif_fnt<100\n");*/
VWDriveTurn(vw_, M_PI/4.0, 0.4);
while(!VWDriveDone(vw_))
prev_ = PSDGet(psd_x[2]) - OFFSET_RIGHT;
while((dist_ = PSDGet(psd_x[2]) - OFFSET_RIGHT) <= prev_){
VWSetSpeed(vw_, 0, 0.30);
prev_ = dist_;
}
}
else{
MAP_Pos(pos_n_, ROBOT_RADIUS, -M_PI/2.0, &x1, &y1);
MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, -M_PI/2.0, &x2, &y2);
/* Condition for boundary limits reached */
if((map_[y1 * GRID_DIMENSIONS + x1] == MAP_BORDER /*5*/) ||
(map_[y2 * GRID_DIMENSIONS + x2] == MAP_BORDER /*5*/))
VWSetSpeed(vw_, 0.15, 0);
/* an obstacle close to right of robot - enact collision avoidance routine */
else if((PSDGet(psd_x[2]) - OFFSET_RIGHT) < 100/*80*/){/* 80 chosen arbitarily */
stop_forward(vw_);
VWSetSpeed(vw_, 0, 0.2);
}
/* No obstacle detected (outward corner) -
* robot must turn right to relocate obstacle */
else if((PSDGet(psd_x[2]) - OFFSET_RIGHT) > 138){/* 38 + 100 = 138 */
VWSetSpeed(vw_, 0.05, -0.55);
dist_ = prev_;
}
/* Veering close to obstacle, adjust trajectory */
else if(dist_ > prev_) VWSetSpeed(vw_, 0.15, -0.2);
/* Veering away from obstacle, adjust trajectory */
else if(dist_ < prev_) VWSetSpeed(vw_, 0.15, 0.2);
/* Robot moving parallel to obstacle - maintain trajectory */
else VWSetSpeed(vw_, 0.15, 0);
}
prev_ = dist_;
MAP_Position(vw_, &pos_n_, &map_[0]);
}
}
}
}while(fabs(pos_n_.x - dest_n.x) + fabs(pos_n_.y - dest_n.y) > 0.16);
//VEHStop(vw_);
stop_forward(vw_);
for(i = -1; i < 2; i++)
for(j = -1; j < 2; j++)
if(map_[target + GRID_DIMENSIONS * i + j] == CLEAR_UNEXP /*6*/)
map_[target + GRID_DIMENSIONS * i + j] = CLEAR; /*0;*/
if(map_[target] == BLOCKED_UNEXP /*4*/) map_[target] = NOT_BORDER; /*3;*/
LCDPutString("end_Navigation\n");
}
/* 11----------------------= MAP_Locate =--------------------------- */
/** Locates the nearest unexplored obstacle to the robot. And proceeds to that
obstacle. The maximum range of operation is the limits to the PSD
set by PSD_MAX.
@param vw_ The VW Handle
@param x_ The PSD Handle for the front PSD sensor
@param map_ The address of the occupation grid system */
int MAP_Locate (VWHandle vw_, PSDHandle x_, char *map_)
{
int mindist_ = PSD_MAX, obstacle_, p_x, p_y, x = 0, y = 0;
radians angle_ = 0;
PositionType pos_n_, stop_;
SpeedType stopped_;
LCDPutString("Locate_obstacle\n");
VEHStop(vw_); /* clear previous speed setting */
OSWait(1); /* wait for a while */
VWDriveTurn(vw_, 2.0 * M_PI, 0.5); /* 0.5- magic number */
LCDPutString("Full circle\n");
while (!VWDriveDone(vw_)){ /* Rotates around a full circle, locating obstacles */
MAP_Position(vw_, &pos_n_, &map_[0]);
MAP_Pos(pos_n_, 0, 0, &p_x, &p_y);
/* LCDPutString("F_circ_turning\n"); */
/*obstacle_ = PSDGet(x_) - OFFSET_FRONT;*/
if((obstacle_ = PSDGet(x_) - OFFSET_FRONT) < 80){/* 80 chosen arbitarily */
obstacle_ = 70; /*30; original=35; magic number */
/* LCDPutString("Obstacle=70\n"); */
}
if(obstacle_ < PSD_MAX){ /* Obstacle detected */
MAP_Pos(pos_n_, obstacle_ + ROBOT_RADIUS, 0, &x, &y);
/* Ensure obstacle is not previously explored */
if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
if((map_[y*GRID_DIMENSIONS+x] != CLEAR /*0*/) && (map_[y*GRID_DIMENSIONS+x] != BLOCKED /*1*/))
if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, BLOCKED /*1*/)){
map_[y * GRID_DIMENSIONS + x] = BLOCKED_UNEXP; /*4;*/
if(obstacle_ < mindist_){
mindist_ = obstacle_;
angle_ = pos_n_.phi;
/* LCDPutString("F_circ_notExpl\n"); */
}
}
}
/* Mark map according for the sensor detecting no obstacles */
else if(obstacle_ >= PSD_MAX){
MAP_Pos(pos_n_, PSD_MAX + ROBOT_RADIUS, 0, &x, &y);
if((x >= 0) && (x < GRID_DIMENSIONS) && (y >= 0) && (y < GRID_DIMENSIONS))
if((map_[y*GRID_DIMENSIONS+x] != CLEAR/*0*/) && (map_[y*GRID_DIMENSIONS+x] != BLOCKED/*1*/))
if(!MAP_Check(&map_[0], y * GRID_DIMENSIONS + x, BLOCKED/*1*/))
map_[y * GRID_DIMENSIONS + x] = CLEAR_UNEXP; /*6;*/
}
}
VEHStop(vw_);
LCDPutString("end_Full_circle\n");
if(mindist_ == PSD_MAX) return NOT_CLOSE; /* 0; */
MAP_Position(vw_, &stop_, &map_[0]);
VEHStop(vw_); /* clear previous speed setting */
OSWait(1); /* wait for a while */
VWDriveTurn(vw_, -MAP_Angle(angle_, stop_.phi), M_PI / 3.0);
while(!VWDriveDone(vw_)); /* LCDPutString("turn_rate=PI/3\n"); */
VEHStop(vw_);
OSWait(1); /* wait for a while */
/* Drive robot to the nearest detected obstacle */
/* =first move robot away from detected obstacle if it too close= */
if(front_too_close(vw_, x_)){
move_backward(vw_, x_);
VEHStop(vw_);
OSWait(1); /* wait for a while */
}
//LCDPutString("Drive towards\n");
/* =then drive robot towards the detected obstacle= */
VWSetSpeed(vw_, 0.10, 0); /* 0.10- magic number */
do{
MAP_Position(vw_, &stop_, &map_[0]);
VWGetSpeed(vw_, &stopped_);
}while((PSDGet(x_) - OFFSET_FRONT) > FRONT_GAP/*90*/ ); /* 90 chosen arbitarily */
stop_forward(vw_);
OSWait(1); /* wait for a while */
return IS_CLOSE; /*1;*/
}
/* 12-------------------- Wall_Follow_Right ------------------------ */
/** A boundary-following algorithm that tracks the right side of the robot.
@param vw_ The VW Handle
@param psd_ The addresses of the Right, Front and Left PSD sensors
@param map The address for the occupation grids
@param segs The address for the configuration space
@param count The counter for the number of line-segments
@see MAP_Navigation */
void Wall_Follow_Right (VWHandle vw_, PSDHandle * psd_, char *map_,
Segment * segs_, int *count_)
{
int dist_ = 0, prev_ = -1, looped_ = 0, val_ = 0, turn_ = 0;
int x1, y1, x2, y2;
radians orient_ = 0;
SpeedType stopped_;
PositionType pos_n_, start_;
/* Rotate robot so that it is approximately parallel to obstacle */
VWDriveTurn(vw_, M_PI / 2.0, 0.4); /* 0.4- magic number */
while(!VWDriveDone(vw_))
val_ = MAP_Dist(vw_, psd_[2], psd_[0], &map_[0]);
VEHStop(vw_);
VWSetSpeed(vw_, 0.2, 0); /* 0.2- magic number */
MAP_Position(vw_, &pos_n_, &map_[0]);
MAP_Position(vw_, &start_, &map_[0]);
if(*count_ < GRID_DIMENSIONS*10){ /* Store configuration space map */
segs_[*count_].x1 = (int) (MAP_Segment(start_.x) +
(val_ + ROBOT_RADIUS) * cos(start_.phi - M_PI/2.0));
segs_[*count_].y1 = (int) (MAP_Segment(start_.y) +
(val_ + ROBOT_RADIUS) * sin(start_.phi - M_PI/2.0));
}
/* Wall-Following Routine */
while((looped_ == 0) || ((fabs(start_.x - pos_n_.x) > 0.1) ||
(fabs(start_.y - pos_n_.y) > 0.1))){ /*0.1- magic number*/
MAP_Pos(pos_n_, ROBOT_RADIUS, 0, &x1, &y1);
MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, 0, &x2, &y2);
VWGetSpeed(vw_, &stopped_);
dist_ = MAP_Dist(vw_, psd_[2], psd_[0], &map_[0]);
/* Condition for robot stall (collision) */
if((stopped_.v < 0.025) && (stopped_.w == 0)) /* 0.025- magic number */
VWSetSpeed(vw_, 0, 0.3); /* 0.3- magic number */
/* Condition for grid boundary limit reached */
else if((map_[y1 * GRID_DIMENSIONS + x1] == MAP_BORDER/*5*/) ||
(map_[y2 * GRID_DIMENSIONS + x2] == MAP_BORDER/*5*/)){
VWSetSpeed(vw_, 0, 0.4); /* 0.4- magic number */
/* Store configuration space map */
if((turn_ == 0) && (*count_ < 10*GRID_DIMENSIONS)){
segs_[*count_].x2 = (int) (MAP_Segment(pos_n_.x) +
(ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2.0) +
turn_ * ROBOT_RADIUS * cos(pos_n_.phi));
segs_[*count_].y2 = (int) (MAP_Segment(pos_n_.y) +
(ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2.0) +
turn_ * ROBOT_RADIUS * sin(pos_n_.phi));
*count_ += 1;
}
turn_ = 1;
} /* Obstacle detected in front of robot - enact left turn to avoid collision */
else if((PSDGet(psd_[1]) - OFFSET_FRONT) <= FRONT_GAP/*GRID_WIDTH*/){
/*VEHStop(vw_);*/
stop_forward(vw_);
VWSetSpeed(vw_, 0, 0.30);
orient_ = pos_n_.phi - M_PI / 5.0;
/* Rotation until robot reaches corner of the boundary */
while((dist_ = MAP_Dist(vw_, psd_[2], psd_[0], &map_[0])) >= prev_){
prev_ = dist_;
if((dist_ <= 120) && (turn_ == 0) && (*count_ < GRID_DIMENSIONS*10)){/*120- magic number*/
MAP_Position(vw_, &pos_n_, &map_[0]);
/* Condition to store configuration space map */
if(MAP_Angle(orient_, pos_n_.phi) >= (M_PI / 12.0)){
orient_ = pos_n_.phi;
segs_[*count_].x2 = (int) (MAP_Segment(pos_n_.x) +
(ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2.0));
segs_[*count_].y2 = (int) (MAP_Segment(pos_n_.y) +
(ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2.0));
*count_ += 1;
segs_[*count_].x1 = (int) (MAP_Segment(pos_n_.x) +
(ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2.0));
segs_[*count_].y1 = (int) (MAP_Segment(pos_n_.y) +
(ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2.0));
}
}
}
orient_ = pos_n_.phi - M_PI / 5.0;
/* Rotation until robot is parallel to new side of obstacle */
while((dist_ = MAP_Dist(vw_, psd_[2], psd_[0], &map_[0])) <= prev_){
prev_ = dist_;
if((dist_ <= 120) && (turn_ == 0) && (*count_ < GRID_DIMENSIONS*10)){/*120- magic number*/
MAP_Position(vw_, &pos_n_, &map_[0]);
/* Condition to store configuration space map */
if(MAP_Angle(orient_, pos_n_.phi) >= (M_PI / 12.0)){
orient_ = pos_n_.phi;
segs_[*count_].x2 = (int) (MAP_Segment(pos_n_.x) +
(ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2.0));
segs_[*count_].y2 = (int) (MAP_Segment(pos_n_.y) +
(ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2.0));
*count_ += 1;
segs_[*count_].x1 = (int) (MAP_Segment(pos_n_.x) +
(ROBOT_RADIUS + dist_) * cos(pos_n_.phi - M_PI/2.0));
segs_[*count_].y1 = (int) (MAP_Segment(pos_n_.y) +
(ROBOT_RADIUS + dist_) * sin(pos_n_.phi - M_PI/2.0));
}
}
}
VEHStop(vw_);
turn_ = 0;
}
else{
MAP_Pos(pos_n_, ROBOT_RADIUS, -M_PI/2.0, &x1, &y1);
MAP_Pos(pos_n_, GRID_WIDTH + ROBOT_RADIUS, -M_PI/2.0, &x2, &y2);
/* Boundary condition reached - robot at end of grid map
* Robot must maintain trajectory to avoid crossing the boundary */
if((map_[y1 * GRID_DIMENSIONS + x1] == MAP_BORDER/*5*/) ||
(map_[y2 * GRID_DIMENSIONS + x2] == MAP_BORDER/*5*/))
VWSetSpeed(vw_, 0.15, 0); /* 0.15- magic number */
/* Right PSD to detect collision with the obstacle */
else if((PSDGet(psd_[2]) - OFFSET_RIGHT) < FOLLOW_GAP/*80*/)/* 80 chose arbitarily */
VWSetSpeed(vw_, 0, 0.2); /* 0.2- magic number */
/* Obstacle not detected beside robot (outward corner) -
* rotate accordingly to locate obstacle */
else if((PSDGet(psd_[2]) - OFFSET_RIGHT) > ADJUST_LIMIT/*95*/){/* 95- magic number */
/* Store the configuration space map */
if((prev_ <= ADJUST_LIMIT/*95*/) && (turn_ == 0) && (*count_ < GRID_DIMENSIONS*10)){/* 95- magic number */
MAP_Position(vw_, &pos_n_, &map_[0]);
segs_[*count_].x2 = (int) (MAP_Segment(pos_n_.x) +
(ROBOT_RADIUS + prev_) * cos(pos_n_.phi - M_PI/2.0));
segs_[*count_].y2 = (int) (MAP_Segment(pos_n_.y) +
(ROBOT_RADIUS + prev_) * sin(pos_n_.phi - M_PI/2.0));
*count_ += 1;
segs_[*count_].x1 = (int) (MAP_Segment(pos_n_.x) +
(ROBOT_RADIUS + prev_) * cos(pos_n_.phi - M_PI/2.0));
segs_[*count_].y1 = (int) (MAP_Segment(pos_n_.y) +
(ROBOT_RADIUS + prev_) * sin(pos_n_.phi - M_PI/2.0));
}
VWSetSpeed(vw_, 0.05, -0.55);
}
/* Veering too far away from obstacle, adjust trajectory */
else if((prev_ != -1) && (dist_ < prev_)) VWSetSpeed(vw_, 0.15, 0.2);
/* Veering too close to obstacle, adjust trajectory */
else if((prev_ != -1) && (dist_ > prev_)) VWSetSpeed(vw_, 0.15, -0.2);
/* Robot is moving parallel to obstacle - maintain trajectory */
else VWSetSpeed(vw_, 0.15, 0);
}
prev_ = dist_;
MAP_Position(vw_, &pos_n_, &map_[0]);
/* Set condition to determine the end of the wall following routine */
if((fabs(start_.x - pos_n_.x)>0.12)||(fabs(start_.y - pos_n_.y)>0.12))/*0.12- magic number*/
looped_ = 1;
}
VEHStop(vw_);
/* Store final point in the configuration space for the obstacle */
if(*count_ < 10 * GRID_DIMENSIONS){
segs_[*count_].x2 = (int) (MAP_Segment(start_.x) +
(ROBOT_RADIUS + val_) * cos(start_.phi-M_PI/2.0));
segs_[*count_].y2 = (int) (MAP_Segment(start_.y) +
(ROBOT_RADIUS + val_) * sin(start_.phi-M_PI/2.0));
*count_ += 1;
}
}
/* 13-----------------------= LCD_MAP =----------------------------- */
/** Draws the occupational grid onto the LCD display. The map is centred on the robot.
@param vw_ The VW Handle
@param map_ The address of the grid
@see Display_MAP UpLoad_Grid */
void LCD_MAP(VWHandle vw_, char *map_)
{
int i = 0, j = 0, x, y;
PositionType pos_n_;
/* Centres the map on the robot position */
VWGetPosition(vw_, &pos_n_);
MAP_Pos(pos_n_, 0, 0, &x, &y);
/* Adjusts map if it encounters the end of the grid boundaries */
if(x < 64) x = 64;
else if(x > (GRID_DIMENSIONS - 64)) x = GRID_DIMENSIONS - 64;
if(y < 32) y = 32;
else if(y > (GRID_DIMENSIONS - 32)) y = GRID_DIMENSIONS - 32;
LCDClear();
/* Draws the map on the LCD Display */
while((j < 64) && ((y + j - 32) < GRID_DIMENSIONS)){
i = 0;
while((i < 128) && ((x + i - 64) < GRID_DIMENSIONS)){
if((x + i - 64) >= 0)
if((y + j - 32) >= 0)
if(map_[(y + j - 32)*GRID_DIMENSIONS+(x + i - 64)] == BLOCKED /*1*/)
LCDSetPixel(j, i, 1);
i++;
}
j++;
}
}
/* 14-----------------------= Display_MAP =------------------------- */
/** Displays the map in ascii format (Used for simulator only).
@param map_ The address of the grids
@see LCD_MAP UpLoad_Grid */
void Display_MAP(char *map_)
{
int i, j;
for(i = 0; i < GRID_DIMENSIONS; i++){
for(j = 0; j < GRID_DIMENSIONS; j++)
switch(map_[i * GRID_DIMENSIONS + j]){
case 0:
fprintf(stderr," "); break;
case 1:
fprintf(stderr,"@"); break;
case 3:
fprintf(stderr,":"); break;
case 4:
fprintf(stderr,"X"); break;
case 5:
fprintf(stderr,"*"); break;
case 6:
fprintf(stderr,"."); break;
default:
fprintf(stderr,"-"); break;
}
fprintf(stderr,"\n");
}
}
/* 15-----------------------= MAP_Target =------------------------- */
/** Locates an unexplored cell and returns the index as the target.
@param map_ The address of the occupational grids */
int MAP_Target(char *map_)
{
int i = 0;
while (i < (GRID_DIMENSIONS*GRID_DIMENSIONS)){
if((map_[i] == CLEAR_UNEXP /*6*/) || (map_[i] == BLOCKED_UNEXP /*4*/))
return i;
i++;
}
return IS_EXPLORED; /*-1;*/
}
/* 16------------------------= UpLoad_VG =-------------------------- */
/** Loads the map through the SERIAL1 link
Loads the configuration space system.
@param values The configuration space address
@param v_count The counter for the configuration space
@see UpLoad_Grid */
void UpLoad_VG(Segment *values, int v_count)
{
char string[50], term = 4;
int i, j, len;
string[0] = '\0';
len = sprintf(string, "width %d\n", GRID_DIMENSIONS * GRID_DIMENSIONS + 100);
for(j = 0; j < len; j++) OSSendRS232(&string[j], SERIAL1);
string[0] = '\0';
len = sprintf(string, "height %d\n", GRID_DIMENSIONS * GRID_DIMENSIONS + 100);
for(j = 0; j < len; j++) OSSendRS232(&string[j], SERIAL1);
for(i = 0; i < v_count; i++){
len = sprintf(string, "%d %d %d %d\n", values[i].x1, values[i].y1,
values[i].x2, values[i].y2);
for(j = 0; j < len; j++) OSSendRS232(&string[j], SERIAL1);
}
OSSendRS232(&term, SERIAL1);
LCDPutString("Map UpLoad complete!\n");
}
/* 17------------------------= UpLoad_Grid =------------------------ */
/** Loads the occupational grids system through the SERIAL2 link.
@param info The address of the grids map representation
@see UpLoad_VG Display_MAP LCD_MAP */
void UpLoad_Grid(char *info)
{
char term = 4, obs = 'X', clr = ' ', nl = '\n';
int i, j;
for(i = 0; i < GRID_DIMENSIONS; i++){
for(j = 0; j < GRID_DIMENSIONS; j++)
if(info[i*GRID_DIMENSIONS + j] == 1) OSSendRS232(&obs, SERIAL2);
else OSSendRS232(&clr, SERIAL2);
OSSendRS232(&nl, SERIAL2);
}
OSSendRS232(&term, SERIAL2);
LCDPutString("UpLoad_Grid complete\n");
}
/* 18========================= main function ======================= */
int main ()
{
/* -------- Variable declarations -------- */
VWHandle vw;
Segment data[GRID_DIMENSIONS*10];
int count = 0, i;
char map[GRID_DIMENSIONS*GRID_DIMENSIONS] = {1};
PSDHandle psd[NUM_PSD];
for(i=GRID_DIMENSIONS;i<GRID_DIMENSIONS*GRID_DIMENSIONS;i++)
map[i] = NOT_BORDER; /*3;*/
for(i=0;i<GRID_DIMENSIONS;i++){
map[i] = MAP_BORDER; /*5;*/
map[GRID_DIMENSIONS*GRID_DIMENSIONS - 1 - i] = MAP_BORDER;
map[GRID_DIMENSIONS*i] = MAP_BORDER;
map[GRID_DIMENSIONS*i + GRID_DIMENSIONS-1] = MAP_BORDER;
}
LCDMode(SCROLLING|NOCURSOR);
/* - - - - - - - PSD Handler - - - - - - - */
psd[0] = PSDInit(PSD_LEFT);
if(psd[0] == 0){
LCDPutString("PSDInit Error!\n");
OSWait(20); exit(1);
}
psd[1] = PSDInit(PSD_FRONT);
if(psd[1] == 0){
LCDPutString("PSDInit Error!\n");
OSWait(20); exit(1);
}
psd[2] = PSDInit(PSD_RIGHT);
if(psd[2] == 0){
LCDPutString("PSDInit Error!\n");
OSWait(20); exit(1);
}
PSDStart(psd[0] + psd[1] + psd[2], TRUE);
/* - - - - init v-omega interface - - - - */
vw = VWInit(VW_DRIVE,1);
if(vw == 0){
LCDPutString("VWInit Error!\n");
OSWait(200); exit(1);
}
VWSetPosition(vw, 0.0, 0.0, 0.0);
VWStartControl(vw,V_LIN,T_LIN,V_ROT,T_ROT);
/* - - Initialize rs232 with given setting - - */
OSInitRS232(SER115200, RTSCTS, SERIAL1);
/************ main action loop ************/
while(TRUE){
LCDPutString("Scanning...\n");
if(!MAP_Locate(vw, psd[1], &map[0])){
i = MAP_Target(&map[0]);
if(i != IS_EXPLORED /*-1*/){
MAP_Navigation(vw, &psd[0], i, &map[0]);
}
else{
LCDPrintf("\nMap Generation Finished\n");
LCDMenu("MAP","GRID","LINE","STOP");
do{
i = KEYRead();
switch (i){
case KEY1: break;
case KEY2:
UpLoad_Grid(&map[0]); break;
case KEY3:
UpLoad_VG(&data[0], count); break;
case KEY4:
PSDRelease();
VWRelease(vw); exit(0);
default: break;
}
}while (i != KEY1);
LCDMenu("","","","");
count = 0;
for(i=GRID_DIMENSIONS;i<GRID_DIMENSIONS*GRID_DIMENSIONS;i++)
map[i] = NOT_BORDER; /*3;*/
for(i=0;i<GRID_DIMENSIONS;i++){
map[i] = MAP_BORDER; /*5;*/
map[GRID_DIMENSIONS*GRID_DIMENSIONS - 1 - i] = MAP_BORDER;
map[GRID_DIMENSIONS*i] = MAP_BORDER;
map[GRID_DIMENSIONS*i + GRID_DIMENSIONS-1] = MAP_BORDER;
}
}
}
else{
LCDPutString("Wall-following\n");
Wall_Follow_Right(vw, &psd[0], &map[0], &data[0], &count);
LCD_MAP(vw, &map[0]);
KEYWait(ANYKEY);
/* LCDMenu("Cont","","","Load");
i = KEYRead();
if(i == KEY4){
LCDMenu("Map","Grid","Ln","");
do{
i = KEYRead();
switch (i){
case KEY1: break;
case KEY2:
UpLoad_Grid(&map[0]); break;
case KEY3:
UpLoad_VG(&data[0], count); break;
default: break;
}
}while (i != KEY1);
}
LCDMenu("Map","","","Stop");
i = KEYRead();
if(i == KEY1){
LCDPutString("Hit a key to go\n");
KEYWait(ANYKEY);
}
else if(i == KEY4){
PSDRelease();
VWRelease(vw); exit(0);
}*/
}
}
} /* end of program */