This page contains screen shots and links for a simple P-Controller line follower and a more complex PID-Controller line follower. The code are RBT files and were created with NXT-G version 1.1. For more info see the PID Controller For Lego Mindstorms Robots page.
Click the image to download the .RBT file. The PID_LF_Calib MyBlock is later on this page.
The PID_LF_Calib MyBlock pivots the robot looking for the maximum and minimum light values. From those values the "gray value" is calculated. PID_LF_Calib can be removed and the gray value simply entered in the "gray" suitcase.
The motor power level is set in the MOTOR block. The higher the motor power level the more difficult it is to follow a line.
This controller is a left-hand edge follower and uses the A motor as the left and C motor as the right motor. The light sensor is assumed to be on port 2.
The proportionality constant (Kp) is in the multiplication block and is entered as 100X the actual value.
The best Kp value will vary depending on the robot's wheels, the range of light values and the height of the light sensor above the mat. This program works best if the light sensor is between 0.25 and 0.5 inch above the mat and the room lights are fairly consistent in all directions.
Steer_LF_2.RBT:
Click the image below to download the .RBT file. The three MyBlocks are later on this page.
This is a full PID controller for a single light sensor line follower. This controller is a left-hand edge follower and uses the A motor as the left and C motor as the right motor. The light sensor is assumed to be on port 2. The MyBlock that actually controls the motors (PID_LF_MotorControl) uses the variables "A-motor" and "B-motor" for the left and right motors respectively. The actual motors controlled are A and C. ("A-motor" and "B-motor" was a poor choice of variable names. They should have been called something like "Left_motor" and "Right_motor".)
The three PID constants (Kp, Ki and Kd) are entered in the corresponding suitcases. In addition to the three K's, the TargLight and TargPower levels need to be entered in their suitcases. The TargLight value is the gray value (average of white and black). The TargPower is the motor power level for both motors when the robot is driving straight.
The PID_LF_Calib measures maximum and minimum light values and calculates the "gray" level. This block could be removed and the gray value entered directly in the TargLight suitcase.
LF_PID MyBlock does the PID calculation. The input is the current error and the output is the motor power level correction (labeled "PID"). The Ks are passed to the LF_PID block as global variables (variables defined in both the main routine and the MyBlock using the same name and data type in both).
PID_LF_MotorControl takes an input PID value (from LF_PID) at it's "Turn" port and the target motor power level (on the "Target" port), calculates the actual power levels for each motor, and sends those values to the motors. If the calculated power level for a motor is negative then the sign is changed and the power is sent to a MOTOR block set to rotate backwards. (The MOTOR block doesn't understand a negative power level as meaning the motor should reverse direction.)
PID_lineFol_4 is the main program. The main loop is set for a fixed number of cycles. At the end of the run the elapsed time in milliseconds (or is it 1/100ths of a second?) is displayed along with the total rotation angle of the A motor. The elapsed time and the number of loops can be used to calculate the loop time (dT) and is useful when calibrating the PID. The time per loop is about 14 milliseconds.
To change the stop condition of the loop will require some additional programing.
Suitable Kp,Ki,Kd, TargPower and TargLight values will need to be determined for your robot. The values in the program are for a light sensor about 1/4" above the mat, 49.6mm diameter wheels, and gray value (white - black) of about 10 to 20. The Kp, Ki and Kd values are all entered as 100 times bigger than they actually are. For example a Kp of 1.25 is entered as 125.
PID_lineFol_4:
These MyBlocks need to be placed in the folder that has your MyBlocks. Probably something like;
c:\My Documents\Lego
Creations\MINDSTORMS Projects\
Profiles\default\Blocks\My Blocks
This MyBlock controls the motors. It takes an input PID value (from LF_PID) at it's "Turn" port and the target motor power level (on the "Target" port), calculates the actual power levels for each motor, and sends those values to the motors. If the calculated power level for a motor is negative then the sign is changed and the power is sent to a MOTOR block set to rotate backwards. (The MOTOR block doesn't understand a negative power level as meaning the motor should reverse direction.)
The NXT will beep anytime a MOTOR block reverses direction. If the follower is working properly (is tuned properly) then the beeps should not occur very often.
LF_PID:
LF_PID MyBlock does the actual PID calculation. The input is the current error and the output is the motor power level correction (labeled "PID"). The Ks are passed to the LF_PID block as global variables (variables defined in both the main routine and the MyBlock using the same name and data type in both).
PID_LF_Calib:
This MyBlock swings the robot's nose through an arc and measures the maximum and minimum light values. Using those values the "gray" level is calculated and returned on the "Result" port. This block could be removed and the gray value entered directly in the TargLight suitcase of the line follower main routine. Click the image to download the .rbt file.
