SteerBot - A 51515 Robot Inventor car that edge tracks a line

[GusJansson]

Description
A robot with car like steering has been one of my favorite exercises in robot building. This project, in fact, is largely a variation of what I have previously done with the LEGO RCX, the NXT and to a lesser extent the EV3. But with the 51515 set I as able to take advantage of improved resolution of the color sensor as well as some cool features of Pybricks to make the main tracking code pretty simple.

The robot features Ackermann steering. This means that the steering linkage is more trapezoidal than parallel. The front wheels are only parallel when the steering is pointing straight ahead. When it turns the inside wheel turns sharper than the outside wheel so the wheels track more accurately since the inside wheel has a tighter turning radius. This is particularly important in order to achieve tight overall turning radius with a relatively wide robot.

The robot uses the Color Sensor mounted on an axle high and attached to the steering motor. This means that in order to track a line, actually just the edge of the line, the tracking code tries to keep the sensor on the edge of the line. If the sensor is on the edge, then as the robot drives it will follow the edge since the robot is now steering to where the edge is in front of the robot. The sensor is mounted high in order to give a gradual reading over the edge so that while near the edge the program can determine where the edge is relative to the sensor.

The driving is done through the super cool 5 gear differential that is included with the set. This is the first time I use this LEGO differential and I love it! The drive motor is actually gearing up with the intention of being able to go really fast. On my tight test track I was never able to make it go full speed without running off the line.

Video
https://www.youtube.com/watch?v=i4WPa-rtFB8&feature=youtu.be

Source Code

# SteerBot for Pybricks
#
# To download on brick:
# in CMD cd to the directory for pybricksdev
# poetry run pybricksdev run usb 'Hub' ../pybricks/steerbot.py
#
# New and improved!
# 11/19/2000 Incorporated tips from Laurens Valk
#   ColorSensor hsv method to get saturation value to track line
#   More elegant steer motor limit control
#   Use control.limits for speed control. Firmware now controls acceleration.


from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor, ColorSensor
from pybricks.parameters import Port, Button, Stop
from pybricks.tools import wait

# Initialize the hub, motors, and sensor
hub = InventorHub()
steer_motor = Motor(Port.A)
drive_motor = Motor(Port.B)
sensor = ColorSensor(Port.C)

def WaitForButton(b):
    # Wait for press
    while b not in hub.buttons.pressed():
        wait (10)
    # and release
    while b in hub.buttons.pressed():
        wait (10)

# Use the color saturation value to track line
def GetLight():
    return (sensor.hsv().s)

def Calibrate():
    global aSteerLimit
    global lMin, lMax, signEdge
    
    # Find the Right and Left hard limits 
    aRightLimit = steer_motor.run_until_stalled(400, then=Stop.BRAKE, duty_limit=100)
    aLeftLimit = steer_motor.run_until_stalled(-400, then=Stop.BRAKE, duty_limit=100)

    # Calculate the steering limit as average of two extremes then reset
    # angle to the negative limit since steering motor is now at neg. extreme
    aSteerLimit = (aRightLimit-aLeftLimit)//2
    steer_motor.reset_angle(-aSteerLimit)
        
    # Scan from -30 to 30 to get min max of light sensor value
    steer_motor.run_target(1000,-30, then=Stop.BRAKE)
    lMin = 1024; lMax = 0
    lLeft = GetLight()
    steer_motor.run(100)
    c = 0
    while steer_motor.angle() < 30:
        c += 1
        l = GetLight()
        if l > lMax: lMax = l
        if l < lMin: lMin = l
        wait(5)
    steer_motor.stop()
    lRight = GetLight()
    # signEdge is positive 1 if left edge and -1 if right edge
    signEdge = 1 if lLeft < lRight else -1
        
    # Center the steering
    steer_motor.run_target(1000,0,then=Stop.BRAKE,wait=False)
    

SPEED_MAX = 1000
SPEED_TURN = 500
SPEED_OFFLINE = 400
        
def TrackSpeedControl():
    global lMin, lMax, signEdge
    global aSteerLimit
    
    lMid = (lMax+lMin+1)//2
    m = 20.0/(lMax-lMid)
        
    # Calculate a threshold to determine steering is not near the edge
    lOffEdgeThresh = (lMax-lMid) * 0.7

    # Set max speed, acceleration, and max power for drive motor
    drive_motor.stop()  # must be stopped to set limits
    drive_motor.control.limits(1000,2000,100)

    while hub.buttons.pressed() == []:
        # Get a new light value and subtract mid to get signed error from edge
        l = signEdge * (GetLight()-lMid)

        # Create a new target for the steering motor to move toward the
        # approximate position of the edge
        a = steer_motor.angle()
        t = a - m*l

        # Clamp the target angle to within +- aSteerLimit
        t = min(t, aSteerLimit)
        t = max(t, -aSteerLimit)

        # Now update target to move toward edge of line        
        steer_motor.track_target(t)
        
        # Speed control
        if abs(l) < lOffEdgeThresh:
            # On edge of line
            if abs(t) < 25:
                # and going straight
                drive_motor.run(SPEED_MAX)
            else:
                drive_motor.run(SPEED_TURN)
        else:
            drive_motor.run(SPEED_OFFLINE)
                
        wait(3)   

    drive_motor.run(0)
    steer_motor.track_target(0)
    wait(200)
    steer_motor.stop()
    drive_motor.stop()
    while not hub.buttons.pressed() == []: pass

def Test():
    while True:
        WaitForButton(Button.RIGHT)
        Calibrate()
        TrackSpeedControl()

Test()

Programming Notes
Originally I was using the reflection() light value from the ColorSensor. With the blue painters tape I had available and with the sensor mounted high in order to get a gradual value as the sensor passed over the edge, I got a rather unsatisfying resolution in the sensor value. Something like 3 for on the tape and 29 when over the white surface off the tape. Along with a lot of variation in value as the sensor bounced up and down on the axle. After talking to Laurens about this I found out I could get the raw sensor values using the LUMPDevice class. A little experimenting and I discovered that the raw Saturation value was perfect. With the Saturation value you are not measuring how much light is coming back into the sensor, but how much color there is. Since my tape is blue, I got a high saturation value over the tape (about 675) and a low value, since there is no color, (about 100) over the white surface.
UPDATE: On the suggestion from Laurens, I'm now using the ColorSensor .hsv method to get the saturation value. This value is an integer percent and in my case I get a value of about 90 over the blue tape and 15 off the tape over the white board. This is plenty of resolution and this value is more stable than the raw LUMPDevice saturation value which had quite a bit of noise in it.

I also had some fun controlling the speed of the robot. My test track is very tight and the width of the boards are only 2 feet (61cm) wide. That means if the robot was driving fast it had no chance to make the turns and stay on the boards. But a slow robot is no fun so I worked on a scheme to go fast when going straight but slow down in turns, especially when the sensor as no longer near the edge of the line. And, as any good driver will tell you, it is best to control the speed smoothly. Gradually speeding up and slowing down.
UPDATE: Initially I was doing this speed control by gradually changing the speed of the robot in my code to accelerate and slow down. Turns out that there is a much better way in Pybricks by using control.limits on the motor. Using the control.limits method I can set the Acceleration to a value that works better with this robot making the power control smooth but still pretty aggressive to go fast around the track. Robot is now a bit faster on the track than what is in the video.

Building instructions
I have created some preliminary building instructions for the SteerBot. These do not show the cable routing but it was not difficult to route cables under and around the Hub.
steerbot.pdf
Port A Steering Motor
Port B Drive Motor
Port C Color Sensor

[dlech]

This is really nice, thanks for sharing!

One thing I was curious about is if you could get away with not having a calibration of the steering since the motors have absolute encoders.

[GusJansson]

Thanks, David! And thanks for Pybricks.
I found that even though my motors are reasonably accurate, the absolute 0 position is within 3 degrees off the mark, that error was enough to have an impact on the steering. My left and right limits are something like 6 degrees of each other. One of my goals was to have very tight turning radius and my test track ended up being pretty much as tight as what my robot could handle. In this case, every degree of additional steering counted!
That said, the tracking code doesn't really care what the steering angle is. It just sets the target for the steering relative to where it thinks the edge of the line is. The hard limits only come into play in that the code prevents setting the target beyond the hard limits in order to reduce strain on the motor.

[ZPhilo]

...and did you try to use LEGO Python to show benefit/drawbacks of Pybricks/LEGO implementations?

[laurensvalk]

This is great, thanks for sharing both the code and the design 😄

Looks like you've already found quite a few fun Pybricks features. Here are a few more tricks you might be interested in for your next creation:

• For some slight extra accuracy in the reset procedure, you can read the return value of run_until_stalled() directly instead of reading angle() again.
• If you then do steer_motor.reset_angle((aRightLimit - aLeftLimit)/2), the 0 point is now the middle and everything is symmetric, simplifying the remaining code.
• Are SetSpeedTarget/SetSpeed made to gradually change the speed? If so, perhaps setting steer_motor.control.limits might be an interesting way to simplify this. This will set the speed and acceleration limits for all run commands for that motor.

[laurensvalk]

I'm curious, is the extra resolution of LUMPDevice making a big difference compared to the hsv method or does this give similar results?

from pybricks.pupdevices import ColorSensor
from pybricks.parameters import Port
from pybricks.tools import wait

sensor = ColorSensor(Port.B)

while True:
    # Gets the HSV value
    color = sensor.hsv()

    # Print saturation
    print(color.s)

    # Wait
    wait(500)

If it's a noticeable/big difference in performance in your application, then others might experience the same. If so, we can probably consider looking at increasing the resolution for s, and v, which are currently integers rounded to 0--100.

[GusJansson]

Thanks for the feedback, Laurens! I will incorporate your suggestions and then update the code here with these updates. I will also investigate the use of reading the saturation from hsv. I think the resolution should be adequate but will test to confirm.

Philo, there are definitely some features of the the Pybricks firmware that I took advantage of but I can't say conclusively that this can't be done as well with LEGO python. While Pybricks still has some rough edges, crashed a few times, and is non trivial to install and setup so you can connect to the hub, it has one HUGE advantage over the LEGO Python and that is the documentation. If it fantastic. Take a look here: https://docs.pybricks.com/en/latest/pupdevices.html and make sure to click on Show Examples.

[GusJansson]

I have created some preliminary building instructions for the SteerBot. These do not show the cable routing but it was not difficult to route cables under and around the Hub.
steerbot.pdf
Port A Steering Motor
Port B Drive Motor
Port C Color Sensor

[ZPhilo]

Nitpicker's dept.: differential is supposed to be grey, not white ;)

[laurensvalk]

This is great @GusJansson --- thanks for sharing.

[GusJansson]

Philo! You are right! I fixed it in the model. It makes me happy that you went through the build. Thanks. Now for getting the wires done I need either go back to trusty old MLCad that I have not used it years or invest the time to learn LDCad. What do you suggest? There is no way to do the wires in Studio, right?

[ZPhilo]

No, so far Studio can't create cables. I would definitely try to learn LDCad: creating flexibles parts with LDCad is almost easy... After that, remains the problem for compatibility between cables generated in LDraw domain and Studio building instructions. With direct import, Studio generates BIs for the cable itself ;)

• Documentation of LDCad is here: http://www.melkert.net/LDCad/docs
• There is an "official" video showing LDCad flexible part creation (pneumatic tubing) here: https://youtu.be/Peu5yqD2iAo
• I made a video showing creation of cables for Spike Prime: https://youtu.be/vDnkkPsyCT4
  ...and when you get more fluent with cables generation, I'll help you to reimport in Studio!

[ninoguba]

Thanks again for sharing this! Looks like a good opportunity for me to try out pybricks and adapt your code to what I'm thinking for "racing line" tracking with LEGO race car models.

[laurensvalk]

With all the aero you've added, I'm counting on at least 2X the speed 🚀 .

[ninoguba]

Gotta build a wind tunnel moc LOL