A simple method of obstacle avoidance in LeJOS, using subsumption.
You will need LeJOS, which is a Java runtime and VM for Lego Mindstorms, along with a Java IDE of your choosing with the toolchain needed to build and send the compiled bytecode to the Mindstorms unit.
-
A Lego Mindstorms differential drive robot, with the following sensors:
- Colour sensor, facing down;
- Sonar sensor, facing the front, for distance detection.
We will be using the subsumption method for this (see: Brooks, "A Robust Layered Control System For A Mobile Robot", 1986 here).
The methods are in the code, but they can be described like this:
| Behaviour | Trigger | Action |
|---|---|---|
| AdvanceForward | Always On | Move To Next Waypoint |
| Obstacle Detect | UltrasonicSensor.getDistance() < 20cm |
Navigate around object |
| ColourSearch | End Of List Of Waypoints | Search for coloured square |
| ColourDetect | ColourSensor.getID() == ColourSensor.COLORS.RED |
Add waypoint at (0, 0) to go back to point of origin (0,0) |
| FinishedBehaviour | Button.RIGHT.isDown() |
Stops DataThread thread so that the CSV file can close without losing information |
The trajectory that the robot has taken, according to the robot (the usual caveats about Relative Localisation apply) gets drawn to the screen, using this:
// summarised from DataThread.java
...
while(1){
// see the source code for more details
LCD.setPixel(((int) odom.getPose().getX() / 3) + 10, ((int) odom.getPose().getY() / 3) + 10, 1);
};
...The coordinates of the robot's position gets written to a CSV file every 250ms, for an effective 4Hz refresh rate, to 'log.csv'.
To interpret this, you can use Excel to draw the trajectory using Charts, or you can use something like R, or matplotlib.