javaeclipsenxtlejos-nxj

LeJOS NXT Programming Sonic Sensor


I am trying to make a NXT Robot that has attached the Ultrasonic Sensor. It has to drive until the distance is 15, and then the engines have to stop. After it stops it has to turn, but it doesn't work.

import lejos.nxt.*;

public class test {
    public static void main(String [] args) throws InterruptedException {
        UltrasonicSensor ultra = new UltrasonicSensor(SensorPort.S1);
        for (int i = 0; i < 5; i++) {       
            try {
                Motor.B.rotate(-1500 , true);
                Motor.C.rotate(-1500 , true);
            } catch (Exception E){}

            while ( ultra.getDistance() < 15 ) {
                Motor.B.backward();
                Motor.C.backward();
            }

            LCD.clear();
            LCD.drawString("Distance : "+ultra.getDistance(), 0, 0);
        } 

        Button.waitForAnyPress();
    }
}

My old code, which also didn't work:

import lejos.nxt.*;

public class test {

public static void main(String [] args) throws InterruptedException {
    UltrasonicSensor ultra = new UltrasonicSensor(SensorPort.S1);
    try {
        Motor.B.rotate(-720);
        Motor.C.rotate(-720);

         } catch (Exception E){}

    for (int i = 0; i < 5; i++)
    {
    LCD.drawString("Distance : "+ultra.getDistance(), 0, i);
              Thread.sleep(2000);
         int maxDistance = ultra.getDistance();
         if (maxDistance < 15){
             Motor.B.stop();
             Motor.C.stop();
         }
    }
    Button.waitForAnyPress();
}
}

Solution

  • Assumptions

    Okay, from the looks of things, your code is probably not doing what you want. (In the future, when writing a question on Stack Overflow, please clarify in detail what the expected behavior is, as well as what erroneous behavior you're seeing. Those are usually the first two questions we would ask of you, anyway.)

    First of all, you're going to want to ensure that your NXT kit has been set up properly, with your two motors on B and C, and your sensor on S1. If this is so, continue reading.

    Code Interpretation

    The motor commands:

    try {
        Motor.B.rotate(-1500, true);
        Motor.C.rotate(-1500, true);
    } catch (Exception E) {}
    

    look like they're valid motor commands... but wait! You're using a two-wheeled robot, with the motors connected to two wheels that point in opposite directions? But you're using the same distance and direction for your motor's limit angle! If your wheels oppose each other, then this will do nothing but make the robot spin in a circle.

    NOTE: Since your motors are configured properly, as written in your comments, ignore this part.

    robot spins in a circle

    If you change the direction of one of the motors by changing the positive to a negative, then you'll have them both working in unison to move your robot forward (or backwards, if you change the wrong one!)

    Also, keep in mind that passing true as the second argument in

    Motor.B.rotate(-1500, true);
    Motor.C.rotate(-1500, true);
    

    makes this function in a very specific fashion, according to the Javadoc (emphasis mine):

    If immediateReturn is true, method returns immediately and the motor stops by itself.

    If any motor method is called before the limit is reached, the rotation is canceled.

    The first sentence means that this does what we want it to: It tells our motor to find the right limit angle by itself, but don't make our program wait for it. However, the second sentence means that if any other motor commands are called, it will stop moving to the given limit angle. Yeah, that's right. Those next few lines make us stop moving the motors and do what they say instead.

    Now, this code is problematic for two reasons:

    while (ultra.getDistance() < 30) {
        Motor.B.backward();
        Motor.C.backward();
    }
    

    First, these commands will IMMEDIATELY stop our previous two motor commands from executing, which basically means the motors will jump straight to going "backwards" and looping until the distance sensor reads greater than or equal to 30. This is actually what we want, but we need a bit more...

    Second, after your sensor reads the distance greater than 30, your motors are never told to stop! So even when your program is showing you the distance, and waiting for your button to be pressed, it'll still be moving!

    A Solution

    Okay, there's a few things that need to change:

    Below is your code, edited to address each of these issues. I've included notes where I've made changes to show you what I've changed.

    import lejos.nxt.*;
    
    public class test {
        public static void main(String [] args) throws InterruptedException {
            UltrasonicSensor ultra = new UltrasonicSensor(SensorPort.S1);
            for (int i = 0; i < 5; i++) {
    
                // No initial motor movement (because it did nothing anyway)
    
                // We change this to approach from either direction.
                while (ultra.getDistance() != 30) {
                    // Check whether it's behind or ahead of it.
                    // Assuming that B- and C- increase distance, and B+ and C+ decrease it (depends on robot configuration).
                    // This is called a feedback loop, by the way.
                    if (ultra.getDistance() < 30) { // Move forward (distance+)
                        Motor.B.backward();
                        Motor.C.backward();
                    } else { // Move backward (distance-)
                        Motor.B.forward();
                        Motor.C.forward();
                    }
                }
    
                // We only get here when the distance is right, so stop the motors.
                Motor.B.stop();
                Motor.C.stop();               
    
                LCD.clear();
                LCD.drawString("Distance : "+ultra.getDistance(), 0, 0);
            }
            Button.waitForAnyPress();
        }
    }
    

    Now, this code isn't perfect; it may have a tendency to oscillate between forward and backward on slippery surfaces (which may turn it slightly to the left or right due to differences in applied torque), or if the sensor misses the correct position and the robot overshoots it.

    This code also doesn't wait until the robot stabilizes at the given position, just until the sensor first reports the correct one. Again, this may result in sliding around a bit if the wheels don't have decent traction, the motors are set to smooth acceleration, or if the motors run at too high of a speed.

    To correct these flaws, you'd need a more advanced type of feedback loop which accounts for acceleration and slip, and you'd need to wait until the robot stabilizes at the correct position for a short period of time before stopping the motors.

    However, this should get you moving in the right direction, so to speak.

    EDIT Corrected drive motor directionality, as specified in the comments.