Maze Navigation in Player Stage with Roomba
- by Scott
Here is my code:
/*
Scott Landau
Robot Lab Assignment 1
*/
// Standard Java Libs
import java.io.*;
// Player/Stage Libs
import javaclient2.*;
import javaclient2.structures.*;
import javaclient2.structures.sonar.*;
// Begin
public class SpinningRobot
{
public static Position2DInterface pos = null;
public static LaserInterface laser = null;
public static void main(String[] args)
{
PlayerClient robot = new PlayerClient("localhost", 6665);
laser = robot.requestInterfaceLaser(0, PlayerConstants.PLAYER_OPEN_MODE);
pos = robot.requestInterfacePosition2D(0,PlayerConstants.PLAYER_OPEN_MODE);
robot.runThreaded (-1, -1);
pos.setSpeed(0.5f, -0.25f);
// end pos
float x, y;
x = 46.0f;
y = -46.0f;
boolean done = false;
while( !done ){
if(laser.isDataReady()) {
float[] laser_data = laser.getData().getRanges();
System.out.println("== IR Sensor ==");
System.out.println("Left Wall Distance: "+laser_data[360]);
System.out.println("Right Wall Distance: " +laser_data[0]);
// if laser doesn't reach left wall, move to detect it
// so we can guide using left wall
if ( laser_data[360] < 0.6f ) {
while ( laser_data[360] < 0.6f ) {
pos.setSpeed(0.5f, -0.5f);
}
} else if ( laser_data[0] < 0.6f ) {
while(laser_data[0<0.6f) { pos.setSpeed(0.5f, 0.5f);
}
}
pos.setSpeed(0.5f, -0.25f);
// end pos?
done = ( (pos.getX() == x) && (pos.getY() == y) );
}
}
}
} // End
I was trying to have the Roomba go continuously at a slight right curve, quickly turning away from each wall it came to close to if it recognized it with it's laser. I can only use laser_data[360] and laser_data[0] for this one robot. I think this would eventually navigate the maze.
However, I am using the Player Stage platform, and Stage freezes when the Roomba comes close to a wall using this code, I have no idea why.
Also, if you can think of a better maze navigation algorithm, please let me know.
Thank you!
