How fast line following works

When using Robotic Invention System, or NXT-G for programming a robot, line following is usually done like this:

If the light is more than 50, turn left, else turn right.

This results in a slow scanning motion. It works fine for a first time, but soon, you’ll want to go faster.

I used to think that you just needed 2 light sensors, one on both sides of the line, so that you could go straight if both where white, and turn towards the one that becomes back. There is a better way.

When the light sensor is on the edge of the line, does it see black or white? In fact it sees a bit of both, so you get something in between. The trick is to think of the line as a gradient, like so.

If you put the NXT in the gray area, you can have a proportional steering function. Light gray means just a bit left, while dark gray means just a bit right.

Proportional, you say? Yes, we can just apply good old PID again!

// Define to which ports the sensor and motors are connected
#define LIGHTSENSOR IN_1
#define LEFT OUT_C
#define RIGHT OUT_A

// Define constants to tweak the algorithm
#define kp 100
#define ki 5
#define kd 30
// And another one to scale the final value
#define scale 10

dseg segment

// Light sensor reading
light word

// target light
target word
high word
low word

// The current error
err sdword
// The previous error
errold sdword
// The integral, all accumulated errors
errint sdword
// The deriviate, the expected next error
errdiff sdword

// Final pid value
pid sdword

// Temporary variable for calculations
temp sdword
temp2 sdword

// power to the motors
leftpower sdword
rightpower sdword

dseg ends

thread main
  // Initialize the light sensor
  SetSensorColorRed(LIGHTSENSOR)

  // Get the time and start turning around
  gettick temp
  add temp temp 3000
  OnFwd(LEFT, 50)
  OnRev(RIGHT, 50)

  // get light sensor reading
  getin light LIGHTSENSOR ScaledValue

  // set high and low to that reading
  mov low light
  mov high light

Circle:
  // Get the light reading
  // if it is more than high, jump to Higher
  // if it is lower than low, jump to Lower
  getin light LIGHTSENSOR ScaledValue
  brcmp LT Lower light low
  brcmp GT Higher light high

  // else check if the time has passed
  // Jump to Done, else go back to Circle
  gettick temp2
  brcmp LT Done temp temp2
  jmp Circle

// set light to the new low
// jump back to Circle
Lower:
  mov low light
  jmp Circle

// set light to the new high
// jump back to Circle
Higher:
  mov high light
  jmp Circle

Done:
  // we now have the max and min light value found
  // calculate the center value
  sub target, high, low
  div target target 2
  add target target low

Forever:
  // Read the sensor and store it in light
  getin light LIGHTSENSOR ScaledValue

  // Substract the actual distance from the target for the current error
  sub err target light // Proportional

  // Add the error to the integral
  add errint errint err // Integral
  mul errint errint 0.8 // multiply by 0.8 to dampen it

  // Sunstract the previous error from error
  // so that we get the speed at which the error changes
  sub errdiff err errold // Derivative
  mov errold err // set the current error as he old error

  mul pid err kp // Apply proportional parameter

  mul temp errint ki // Apply integral parameter
  add pid pid temp

  mul temp errdiff kd // Apply derivative parameter
  add pid pid temp

  div pid, pid, scale       // Apply scale

  NumOut(0,0,target)
  NumOut(0,8,light)

  // saturate over 100 and under -100
  brcmp LT, under100, pid, 100
  mov pid, 100
under100:
  brcmp GT, overMin100, pid, -100
  mov pid, -100
overMin100:

  // subtract pid from one of the motors
  brtst LT, Negative, pid
  OnFwd(LEFT, 100)
  sub rightpower 100 pid
  OnFwd(RIGHT, rightpower)

  jmp Run
Negative:
  OnFwd(RIGHT, 100)
  add leftpower 100 pid
  OnFwd(LEFT, rightpower)
Run:

  jmp Forever
endt

Did you know that even the motors of the NXT use PID themselves to provide accurate control?

Plotter

When I got the Ultimate Builders set, it came with instructions for a plotter. Mind you, the RCX had no integrated rotation sensors, so it used a lever rotating against a touch sensor. It even used a complicated construction to drive a pneumatic pump with one motor.

It came with a program to write “LEGO”, which I did not understand, because it was huge. And it didn’t work either, but that turned out to be because of a broken sensor… after I took the plotter apart.

I just found the video of a “Lego Master builder” introducing the model.

Anyway, a plotter was on of the things on my list that had to happen properly someday. The one you see here is my third attempt. The first two where too fast, too bulky, and had a very feeble arm holding the pen.

I actually made this model before the NXTbike, but I messed up the building instructions, so I had to do it again. I can’t recommend Lego Digital Designer for any serious Technic building, use LDraw instead.

The first thing I did, even before my first attempt, was writing a virtual plotter, so I could quickly code up the alphabet. I wrote the software in Python, so that I could use the Turtle module for the virtual printer, and nxt-python for the real job. The software can be found on Github.

I made this model before I realized I should minimize the use of non-NXT parts. Ironically, this model uses a few parts from the Ultimate Builder set, but easy workarounds exist for most parts. Check the parts list before you buy.

Download building instructions

About PID control

I found this video on the blog of Xander Soldaat:

Unfortunately, he does not show how to actually implement a PID controller, or how to tweak the values of the algorithm, so I thought I’d show you how it’s done.

For my robot, I chose the trike base by HiTechnic, because it is simple, and usable for my next program. The result:

If you are new to NBC, the main thing to remember that an action consist of a line, starting with the action, usually followed by the variable to store the result in, followed by other parameters.

add result 1 2

Another important concept are comments, which start with //. These are my notes about what the code does, to help you understand it.

If you want to know more about NBC, read this tutorial.

// Define to which ports the sensor
// and motors are connected
#define ULTRASONICSENSOR IN_4
#define motors OUT_AC

// Define constants to tweak the algorithm
#define kp 50
#define ki 12
#define kd 2
// And another one to scale the final value
#define scale 10

// target distance in cm
#define target 30

// From here to dseg ends are variable declarations
dseg segment

// Ultrasonic sensor reading
distance word

// The current error
err sdword
// The previous error
errold sdword
// The integral, all accumulated errors
errint sdword
// The deriviate, the expected next error
errdiff sdword

// Final pid value
pid sdword

// Temporary variable for calculations
temp sdword

dseg ends

// This is where the actual code starts
thread main
  // Initialize the ultrasonic sensor
  SetSensorUltrasonic(ULTRASONICSENSOR)

Forever:
  // Read the sensor and store it in distance
  ReadSensorUS(ULTRASONICSENSOR, distance)

  // Substract the actual distance
  // from the target for the current error
  sub err target distance // Proportional

  // Add the error to the integral
  add errint errint err // Integral
  mul errint errint 0.8 // multiply by 0.8 for damping

  // Sunstract the previous error from error
  // so that we get the speed
  // at which the error changes
  sub errdiff err errold // Derivative
  // set the current error as he old error
  mov errold err

  mul pid err kp // Apply proportional parameter

  mul temp errint ki // Apply integral parameter
  add pid pid temp

  mul temp errdiff kd // Apply derivative parameter
  add pid pid temp

  div pid, pid, scale       // Apply scale

  ClearScreen()
  NumOut(0,0,pid)
  NumOut(0,16,distance)

  // saturate over 100 and under -100
  brcmp LT, under100, pid, 100
  mov pid, 100
under100:
  brcmp GT, overMin100, pid, -100
  mov pid, -100
overMin100:

  // Turn the motors according to the scaled PID value.
  OnRev(motors, pid)
  jmp Forever
endt

If you have built a robot, and written the PID controller, the last thing you need to do is tweak the parameters on the lines that start with #define.

kp is multiplied by the proportial, this is where you start. Set the other two to zero, and this one to any value.

If the robot does not move, increase it. If the robot oscillates wildly, decrease it. Do this until it until it oscillates just a bit.

Now divide kp roughly in half, so that it does not oscillate, but stops to early. Now increase ki until it reaches the target as fast as needed. It will overshoot its target.

Finally, increase kd until it stops on target with as little oscillation as possible. You might need to go back and tweak the other parameters a bit.

Leave a comment if you have any questions.

NXTbike

I designed this robot to experiment with so-called “single track vehicle dynamics”, or in other words, balancing on a bike.

The challenge with designing a motorcycle like this is keeping he wheelbase short, and positioning the steering motor in a sturdy way.

The program for this robot is based on the principle of “steer into fall”, which means that if the bike leans over to the right, it needs to steer right to correct that.

A problem that I have with this robot is detecting the angle of the robot. The ultrasonic sensor is not precise enough and my floor not uniform enough to use the light sensor, like the NXTway does.

If you have a very uniform floor, you could use my code, but it is probably best to get a HiTechnic gyro with the software from this guy:

If you want to build this model, you need some extra wheels, check the parts list.

Download building instructions

Automatic Gearbox

A while back I was making some large LEGO vehicle of sorts, and I was faced with the choice of gearing down my motor or dedicating a whole extra motor to controlling a gearbox.

I chose to do neither, and build an automatic one. The ones I found where to big and relied on friction and differentials. After a few email exchanges and iterations, I arrived at this compact 2-gear automatic gearbox.

The gear has 2 sides, one with a 1:1 ratio, the other with a 12:20 slowdown.

When no torque is applied, the front lever is pressed onto the 1:1 chain by a rubber band, causing it to turn faster than the 12:20 side, and thus pushing the rear lever up.

When torque is applied, the front lever is pushed up by the force, and starts to slip, making the 12:20 chain turn faster, causing the rear lever to fall back into position.

Automatic Gearbox Building Instructions

LEGO TECHNIC Design School

I found some lost and forgotten Lego building lessons. I think you’ll love them.

Over at Lugnet, in an old thread, someone asked for tips and tricks about studless building(using smooth beams rather than the classic Lego bricks). Someone linked him to the “LEGO TECHNIC Design School”, which sounded really good, but unfortunately, the links where dead.

I searched, and searched, but LEGO just seems to have removed them. Are they to good for this world? Finally, I found them using the Wayback Machine. Enjoy!

Unfortunately, a few images are missing, but it’s still interesting to read.

Diagonal Connections

One thing they teach that I have not seen “in the wild” much are all sorts of diagonal connections, like this 1:2 gear ratio and right-angle connection.

It is possible to build all integer Pythagorean triangles, although few of them are practical.

It might at first be confusing to build a triangle with sides of 3, 4 and 5 in length using beams of 4, 5 and 6 of length, but think of it like this:

What would be the length of a dot? Zero, right? So when talking about LEGO units, a beam with one hole also has a length of zero. Start counting at zero, and it’ll all make sense.

Gamepad remote control

I had this pincer bot that I had not yet programmed, but using software remotes proved disappointing. With a few lines of code, I was able to use any gamepad or joystick to control the robot.

To use this code, you need to know how to execute commands on your computer. Then, do the following.

  1. Install Python if you don’t already have it.
  2. Install PyGame for interfacing with the gamepad.
  3. Install NXT-python for interfacing with the NXT.
  4. Make sure your NXT and gamepad are connected and working.
  5. Run
    python nxtjoy.py

    in the directory where you’ve downloaded the code below.

The code assumes you have the pincer bot in the video, for which I’ll give you instructions later. Moving around should work with most tank-steered robots.
import pygame
from nxt import locator, motor
from time import sleep

# edit this to reflect your joystick axis and buttons
axis = {'x':0, 'y':1}

b = locator.find_one_brick()

left = motor.Motor(b, motor.PORT_B)
right = motor.Motor(b, motor.PORT_A)
action = motor.Motor(b, motor.PORT_C)

closed = False

def limit(nr):
    if nr > 50 or nr < -50:
        return min(127, max(-128, nr))
    else:
        return 0

def move(fwd=0, turn=0):
    lp = int((fwd - turn) * -100)
    rp = int((fwd + turn) * -100)
    left.run(limit(lp))
    right.run(limit(rp))

def pincer(button):
    global closed
    try:
        if button and not closed:
            closed = True
            action.turn(-40, 70, emulate=False)
        elif not button and closed:
            closed = False
            action.turn(30, 70, emulate=False, brake=False)
    except motor.BlockedException:
        print action.get_tacho()

pygame.init()
j = pygame.joystick.Joystick(0) # first joystick
j.init()
print 'Initialized Joystick : %s' % j.get_name()
try:
    while True:
        pygame.event.pump()
        sleep(0.1)

        # get_axis returns a value between -1 and 1
        move(j.get_axis(axis['y']), j.get_axis(axis['x']))
        pincer(j.get_button(0))

except KeyboardInterrupt:
    j.quit()