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?