 | Code:
#pragma config(StandardModel, "REMBOT")
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//GLOBAL VARIABLES grid world dimensions
const int x_size = 10;
const int y_size = 5;
//GLOBAL ARRAY representation of grid world using a 2-Dimensional array
//0 = open space
//1 = barrier
//2 = goal
//99 = robot
int map[x_size][y_size] =
{{0,0,0,0,0},
{0,1,99,1,0},
{0,1,1,1,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,0,0,0},
{0,0,2,0,0},
{0,0,0,0,0},
{0,0,0,0,0}};
//FUNCTION move forward for a variable number of grid blocks
void moveForward(int blocks)
{
//convert number of blocks to encoder counts
//wheel circumference = 17.6 cm
//one block = 20 cm / 35.cm
int countsToTravel = (20/17.6)*(360)*blocks;
//encoder target for countsToTravel
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;
nMotorEncoderTarget[motorB] = countsToTravel;
nMotorEncoderTarget[motorC] = countsToTravel;
motor[motorB] = 50;
motor[motorC] = 50;
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle) {}
//stop for half second at end of movement
motor[motorB] = 0;
motor[motorC] = 0;
wait1Msec(500);
}
//FUNCTION left point turn 90 degrees
void turnLeft90()
{
//distance one wheel must travel for 90 degree point turn = 10.68 cm
//wheel circumference = 17.6 cm
int countsToTravel = (8.6/17.6)*(360);
//encoder target for countsToTravel
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;
nMotorEncoderTarget[motorB] = countsToTravel;
nMotorEncoderTarget[motorC] = countsToTravel;
motor[motorB] = 50;
motor[motorC] = -50;
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle) {}
//stop for half second at end of movement
motor[motorB] = 0;
motor[motorC] = 0;
wait1Msec(500);
}
//FUNCTION right point turn 90 degrees
void turnRight90()
{
//distance one wheel must travel for 90 degree point turn = 10.68 cm
//wheel circumference = 17.6 cm
int countsToTravel = (8.6/17.6)*(360);
//encoder target for countsToTravel
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;
nMotorEncoderTarget[motorB] = countsToTravel;
nMotorEncoderTarget[motorC] = countsToTravel;
motor[motorB] = -50;
motor[motorC] = 50;
while(nMotorRunState[motorB] != runStateIdle && nMotorRunState[motorC] != runStateIdle) {}
//stop for half second at end of movement
motor[motorB] = 0;
motor[motorC] = 0;
wait1Msec(500);
}
//FUNCTION print wavefront map to NXT screen
void PrintWavefrontMap()
{
for(int y=0; y < y_size; y++)
{
string printRow = "";
string buffer = "";
for(int x=0; x < x_size; x++)
{
if(map[x][y] == 99)
{
sprintf(buffer,"%s%c",printRow,'R');
printRow = buffer;
}
else if(map[x][y] == 2)
{
sprintf(buffer,"%s%c",printRow,'G');
printRow = buffer;
}
else if(map[x][y] == 1)
{
sprintf(buffer,"%s%c",printRow,'X');
printRow = buffer;
}
else if(map[x][y] == '*')
{
sprintf(buffer,"%s%c",printRow,'*');
printRow = buffer;
}
else
{
sprintf(buffer,"%s%d",printRow, map[x][y]);
printRow = buffer;
}
}
nxtDisplayString(y, printRow);
}
}
//FUNCTION wavefront algorithm to find most efficient path to goal
void WavefrontSearch()
{
int goal_x, goal_y;
bool foundWave = true;
int currentWave = 2; //Looking for goal first
while(foundWave == true)
{
foundWave = false;
for(int y=0; y < y_size; y++)
{
for(int x=0; x < x_size; x++)
{
if(map[x][y] == currentWave)
{
foundWave = true;
goal_x = x;
goal_y = y;
if(goal_x > 0) //This code checks the array bounds heading WEST
if(map[goal_x-1][goal_y] == 0) //This code checks the WEST direction
map[goal_x-1][goal_y] = currentWave + 1;
if(goal_x < (x_size - 1)) //This code checks the array bounds heading EAST
if(map[goal_x+1][goal_y] == 0)//This code checks the EAST direction
map[goal_x+1][goal_y] = currentWave + 1;
if(goal_y > 0)//This code checks the array bounds heading SOUTH
if(map[goal_x][goal_y-1] == 0) //This code checks the SOUTH direction
map[goal_x][goal_y-1] = currentWave + 1;
if(goal_y < (y_size - 1))//This code checks the array bounds heading NORTH
if(map[goal_x][goal_y+1] == 0) //This code checks the NORTH direction
map[goal_x][goal_y+1] = currentWave + 1;
}
}
}
currentWave++;
PrintWavefrontMap();
wait1Msec(500);
}
}
//FUNCTION follow most efficient path to goal
//and update screen map as robot moves
void NavigateToGoal()
{
//Store our Robots Current Position
int robot_x, robot_y;
//First - Find Goal and Target Locations
for(int x=0; x < x_size; x++)
{
for(int y=0; y < y_size; y++)
{
if(map[x][y] == 99)
{
robot_x = x;
robot_y = y;
}
}
}
//Found Goal and Target, start deciding our next path
int current_x = robot_x;
int current_y = robot_y;
int current_facing = 0;
int next_Direction = 0;
int current_low = 99;
while(current_low > 2)
{
current_low = 99; //Every time, reset to highest number (robot)
next_Direction = current_facing;
int Next_X = 0;
int Next_Y = 0;
//Check Array Bounds West
if(current_x > 0)
if(map[current_x-1][current_y] < current_low && map[current_x-1][current_y] != 1) //Is current space occupied?
{
current_low = map[current_x-1][current_y]; //Set next number
next_Direction = 3; //Set Next Direction as West
Next_X = current_x-1;
Next_Y = current_y;
}
//Check Array Bounds East
if(current_x < (x_size -1))
if(map[current_x+1][current_y] < current_low && map[current_x+1][current_y] != 1) //Is current space occupied?
{
current_low = map[current_x+1][current_y]; //Set next number
next_Direction = 1; //Set Next Direction as East
Next_X = current_x+1;
Next_Y = current_y;
}
//Check Array Bounds South
if(current_y > 0)
if(map[current_x][current_y-1] < current_low && map[current_x][current_y-1] != 1)
{
current_low = map[current_x][current_y-1]; //Set next number
next_Direction = 2; //Set Next Direction as South
Next_X = current_x;
Next_Y = current_y-1;
}
//Check Array Bounds North
if(current_y < (y_size - 1))
if(map[current_x][current_y+1] < current_low && map[current_x][current_y+1] != 1) //Is current space occupied?
{
current_low = map[current_x][current_y+1]; //Set next number
next_Direction = 0; //Set Next Direction as North
Next_X = current_x;
Next_Y = current_y+1;
}
//Okay - We know the number we're heading for, the direction and the coordinates.
current_x = Next_X;
current_y = Next_Y;
map[current_x][current_y] = '*';
//Track the robot's heading
while(current_facing != next_Direction)
{
if (current_facing > next_Direction)
{
turnLeft90();
current_facing--;
}
else if(current_facing < next_Direction)
{
turnRight90();
current_facing++;
}
}
moveForward(1);
PrintWavefrontMap();
wait1Msec(500);
}
}
task main()
{
WavefrontSearch(); //Build map of route with wavefront algorithm
NavigateToGoal(); //Follow most efficient path to goal
wait1Msec(5000); //Leave time to view the LCD screen
}
| |
