In this article I will explain the theory and initial code to get a Lego Spike Prime robot following a wall.
Disclaimer: This article will use Robocup Junior Australia‘s Rescue (Maze) Challenge with a Lego Spike Prime robot as an example.
This article is based on a Spike Prime robot with an Ultrasonic Sensor in port A pointing to the left side and a colour sensor in port B and two motors in ports C and D. You will also need a touch sensor for detecting dead ends. An example design is shown in the article below:
Read on for the steps to get your robot following a wall.
The Theory
Let’s start by placing the robot in the maze course with the colour sensor over the centre of a coloured victim in the centre of the maze tile. Now take a reading from the Ultrasonic sensor in cm, this is the desired distance we want the robot to maintain when following the wall, our Set Point. We can then create a differential value between the Set Point (assume 8cm) and the current Ultrasonic sensor reading.
Example 1: Robot positioned with Ultrasonic reading 8cm
Differential = Set Point – Ultrasonic = 8 – 8 = 0
Example 2: Robot positioned with Ultrasonic reading 10cm
Differential = Set Point – Ultrasonic = 8 – 10 = -2
Example 3: Robot positioned with Ultrasonic reading 6cm
Differential = Set Point – Ultrasonic = 8 – 6 = 2
This Differential value can be used as the basis for controlling the steering of the robot, but needs to be multiplied by a Factor to get it to a large enough value to effectively control the steering. The factor needs to be adjusted to a value that allows the robot to correct its distance from the wall while remaining smooth. A good starting point is a value of 6. Testing can then see whether it should be changed.
This approach works well when following a wall, but when the wall changes direction away from the robot and the Ultrasonic sensor’s reading suddenly jumps up by 30cm or more, the steering value will also jump up and cause the robot to spin on the spot. To solve this issue, we need to implement a maximum steering value that controls how the robot will go around corners. We can use a Max Steer variable and a couple of If statements to limit the upper and lower limits that are used to control steering. The Max Steer value should be set so the robot is the correct distance from the wall after turning a corner, try 30 as a starting value.
The Program
I have seen programs created with lots of Events blocks and want to highly recommend against this approach. The issue is two fold; having lots of events divides the processing power of the controller by splitting the program into multiple flows all running at the same time, and you have no control as events could fire in the middle of other code you are running. Having a single When Program Starts event is all that is needed to create a single program flow that is easier to write and debug.
After the program starts you first want to define settings and initialize variables, then create a main loop for your program. In that loop you can then create My Blocks for each of the different challenges: Wall Follow, Dead End, Black Hole, Victims, and Silver Tile.
Error = (Set Point – Ultrasonic Sensor)
Steer = (Error * Factor)
If Steer > Max Steer then Steer = Max Steer
If Steer < -Max Steer then Steer = -Max Steer
Putting that all together gets you this program as a starting point:
You can experiment with speed, Max Steer and Factor to get the best results for your robot. But remember that reliability is more important than speed. The faster you go, the higher the chance of failing to follow the wall correctly or missing victims.
Also, as you add more code for the other My Blocks, the wall follow will get less sensitive, and you might need to slow down the robot’s speed.
Note: If the robot goes backwards, change the sign on the speed setting. If the robot has a right facing Ultrasonic sensor, change the Error calculation to Ultrasonic – Set Point.
Next Steps
The next steps will be to code the My Blocks to look for and then handle dead ends and black holes. Then you need to detect and count victims and watch for returning to the silver tile to finish and display the victim counts.
Note: Make sure you turn on the More Sensors option as you will need to use the Raw Red colour sensor mode to be able to differentiate between white and silver.
More Information
For more information on robotics, check out the following links:
- David’s Robotics Portal
- #Robotics Demo Video and Hints: Mighty Maisy Maze Rescue
- Robocup Junior Australia – Rescue Maze Resources
Hope this information is useful.
David
This article was originally posted on https://www.winthropdc.com/blog.