Wishful Coding

Didn't you ever wish your
computer understood you?

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 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

  // 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

  // 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
  mov low light
  jmp Circle

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

  // 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

  // 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


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

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

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

  jmp Forever

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