PIDLineFollower.java
import lejos.nxt.*;
 
public class PIDLineFollower {
    public static void main(String[] aArg) throws Exception {
        final int power = 80;
 
        BlackWhiteSensor sensor = new BlackWhiteSensor(SensorPort.S1);
 
        sensor.calibrate();
 
        LCD.clear();
        LCD.drawString("Light: ", 0, 2);
 
        int Kp = 35; // REMEMBER we are using Kp*100 so this is really 10//
        int Ki = 0; // REMEMBER we are using Ki*100 so this is really 1//
        int Kd = 0; // REMEMBER we are using Kd*100 so this is really 100!
        int offset = sensor.getThreshold(); // Initialize the variables
        int Tp = 60;
        int integral = 0; // the place where we will store our integral
        int lastError = 0; // the place where we will store the last error value
        int derivative = 0; // the place where we will store the derivative
        int lightValue, error, turn, powerL, powerR;
 
        while (!Button.ESCAPE.isPressed()) {
 
            lightValue = sensor.read(); // what is the current light reading?
            error = lightValue - offset; // calculate the error by subtracting
                                            // the offset
            integral = integral + error; // calculate the integral
            derivative = error - lastError; // calculate the derivative
            turn = Kp * error + Ki * integral + Kd * derivative; // the "P term"
                                                                    // the
                                                                    // "I term"
                                                                    // and the
                                                                    // "D term"
            turn = turn/100;
            LCD.drawInt(turn, 10, 0, 0);
 
            powerL = Tp + turn; // the power level for the left motor
            powerR = Tp - turn; // the power level for the right motor
            Car.forward(powerR, powerL); // actually issue the command in a
                                            // MOTOR block
            lastError = error; // save the current error so it can be the
                                // lastError next time around
 
        }
 
        Car.stop();
        LCD.clear();
        LCD.drawString("Program stopped", 0, 0);
        LCD.refresh();
    }
}
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License