Super fast line following robot with Ackermann steering.
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.
from pybricks.hubs import InventorHub from pybricks.pupdevices import Motor, ColorSensor from pybricks.parameters import Port, Button, Stop from pybricks.tools import wait # Tuning parameters SPEED_MAX = 1000 SPEED_TURN = 500 SPEED_OFFLINE = 400 # Initialize the hub, motors, and sensor hub = InventorHub() steer_motor = Motor(Port.A) drive_motor = Motor(Port.B) sensor = ColorSensor(Port.C) def wait_for_button(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 get_light(): return sensor.hsv().s def calibrate(): global a_steer_limit global l_min, l_max, sign_edge # Find the Right and Left hard limits a_right_limit = steer_motor.run_until_stalled( 400, then=Stop.BRAKE, duty_limit=100 ) a_left_limit = 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 negative extreme a_steer_limit = (a_right_limit - a_left_limit) // 2 steer_motor.reset_angle(-a_steer_limit) # Scan from -30 to 30 to get min max of light sensor value steer_motor.run_target(1000, -30, then=Stop.BRAKE) l_min = 1024 l_max = 0 l_left = get_light() steer_motor.run(100) while steer_motor.angle() < 30: light = get_light() if light > l_max: l_max = light if light < l_min: l_min = light wait(5) steer_motor.stop() l_right = get_light() # sign_edge is positive 1 if left edge and -1 if right edge sign_edge = 1 if l_left < l_right else -1 # Center the steering steer_motor.run_target(1000, 0, then=Stop.BRAKE, wait=False) def track_speed_control(): l_mid = (l_max + l_min + 1) // 2 m = 20.0 / (l_max - l_mid) # Calculate a threshold to determine steering is not near the edge l_off_edge_thresh = (l_max - l_mid) * 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 not any(hub.buttons.pressed()): # Get a new light value and subtract mid to get signed error # from edge light = sign_edge * (get_light() - l_mid) # Create a new target for the steering motor to move toward # the approximate position of the edge a = steer_motor.angle() t = a - m * light # Clamp the target angle to within +- a_steer_limit t = min(t, a_steer_limit) t = max(t, -a_steer_limit) # Now update target to move toward edge of line steer_motor.track_target(t) # Speed control if abs(light) < l_off_edge_thresh: # 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 any(hub.buttons.pressed()): wait(10) while True: wait_for_button(Button.RIGHT) calibrate() track_speed_control()
This project was submitted by Gus Jansson.