Maze Navigation in Player Stage with Roomba

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();开发者_如何学Python

                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!

It seems like you get stuck against a wall because you are oscillating around distance 0.6.

For instance:

1. Drive angling toward wall, eventually reaching < 0.6f
2. Rotate away from wall, stop when > 0.6f. In other words 0.6000001f (for instance)
3. Resume normal trajectory.
4. Almost immediately < 0.6f
5. Goto 2

You will get stuck angling slightly toward and slightly away from the wall.

You need a buffer zone. Once you get within MIN_DIS distance from the wall you need to rotate away until you reach some BUFFER_DIS from the wall where BUFFER_DIS > MIN_DIS by a fair amount.

EDIT:

Looks like you have a few issues. First you are checking if the laser_data is close to a wall, and if it is you are rotating toward the wall. You need to check if it is NOT close to a wall, and rotate toward it.

Second, you are in a very tight while loop, adjusting wheel speeds. This will probably result in bad things. You need to call a trackWall() function when you are too far from the wall, which will poll laser.isDataReady() in a separate loop. Either that, or you need a state-machine in your main polling loop. If not, you are pointlessly adjusting wheel speed with stale laser data.

Third, can you guarantee that the wall will always be within 0.6f? If not, your robot will spin infinitely if it gets to far from the wall.