LeJOS NXT Programming Sonic Sensor

Question

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();
}
}

What do you mean by "it doesn't work"? Is there an exception or error produced, or does the robot do something unexpected? — helencrump, Sep 22 '15 at 13:34
I am sorry, that was really a weird description of it.. The problem is that it does detect the wall but it doesn't stop driving if the limit 15 is reached. It's like this: If the sensor pings < 15 it stops the engines and turns arround. It has to do that exact thing. But it won't do it.. — lucafj2j282j, Sep 22 '15 at 13:37
I don't see anything related to 15 anything here. All I see is something that'll back up until distance >= 30. — Dave Newton, Sep 22 '15 at 13:39
I am sorry, I got confused with the 1500, I meant 30 indeed. — lucafj2j282j, Sep 22 '15 at 13:40
It's been a while since I used LeJOS, so I might be wrong, but won't Motor.x.backward() just keep going until you explicitly tell it to stop? — helencrump, Sep 22 '15 at 13:42
That's true, but when using rotate the engines go one by one and I know, using try catch will resolve that problem but it really doesn't.. — lucafj2j282j, Sep 22 '15 at 13:47
I've dissected what you have and constructed a possible solution below. — Shotgun Ninja, Sep 22 '15 at 14:35
@Lucafraser Using try-catch watches for exceptions, which indicate a problem with the code, not the state of the motor. I don't believe they do what you think they do. What happens is using `rotate(limit)` **blocks** the current thread, which means the program doesn't move on from that function until it's done moving the motor to that limit. `rotate(limit, true)` is **non-blocking**, meaning it happens in the background and lets the code move ahead. — Shotgun Ninja, Sep 22 '15 at 14:37

score 4 · Accepted Answer · edited Jun 20 '20 at 09:12

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.

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:

Your initial motor command is being blocked out by the later commands that tell it to move to the correct position.
Your motors don't stop when they're supposed to.

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.

LeJOS NXT Programming Sonic Sensor

1 Answers1

Assumptions

Code Interpretation

A Solution