Sejway.java
import lejos.nxt.*;
 
public class Sejway implements ButtonListener {
 
    // PID constants
    final float KP = 28;
    final float KI = 4;
    final float KD = 33;
    final int SCALE = 18;
 
    // Global vars:
    int offset;
    float prev_error;
    float int_error;
 
    LightSensor ls;
 
    public Sejway() {
        ls = new LightSensor(SensorPort.S3, true);
        Button.LEFT.addButtonListener(this);
        Button.RIGHT.addButtonListener(this);
    }
 
    /**
     * 
     */
    public void getBalancePos() {
        // Wait for user to balance and press orange button
        LCD.drawString("offset: ", 0, 0);
        while (!Button.ENTER.isPressed()) {
            // NXTway must be balanced.
            offset = ls.readNormalizedValue(); // 1023 - port.readRawValue()
            LCD.drawInt(offset, 8, 0);
            LCD.refresh();
        }
    }
 
    public void pidControl() {
        LCD.drawString("error: ", 0, 1);
        while (!Button.ESCAPE.isPressed()) {
            int normVal = ls.readNormalizedValue();
 
            // Proportional Error:
            float error = normVal - offset;
            LCD.drawInt((int) error, 8, 1);
            // Adjust far and near light readings:
            if (error < 0) error = error * 1.8F;
 
            // Integral Error:
            int_error = ((int_error + error) * 2)/3;
 
            // Derivative Error:
            float deriv_error = error - prev_error;
            prev_error = error;
 
            float pid_val = (KP * error + KI * int_error + KD * deriv_error) / SCALE;
 
            if (pid_val > 100)
                pid_val = 100;
            if (pid_val < -100)
                pid_val = -100;
 
            // Power derived from PID value:
            int power = (int) Math.abs(pid_val);
            power = 55 + (power * 45) / 100; // NORMALIZE POWER
 
            if (pid_val > 0) {
                MotorPort.B.controlMotor(power, 1);    //Motor.B.forward();
                MotorPort.C.controlMotor(power, 1); //Motor.C.forward();
            } else {
                MotorPort.B.controlMotor(power, 2);    //Motor.B.backward();
                MotorPort.C.controlMotor(power, 2); //Motor.C.backward();
            }
        }
    }
 
    public void shutDown() {
        // Shut down light sensor, motors
        MotorPort.B.controlMotor(0, 4);
        MotorPort.C.controlMotor(0, 4);
        ls.setFloodlight(false);
    }
 
    public static void main(String[] args) {
        Sejway sej = new Sejway();
        sej.getBalancePos();
        sej.pidControl();
        sej.shutDown();
    }
 
    @Override
    public void buttonPressed(Button arg0) {}
 
    @Override
    public void buttonReleased(Button b) {
        if (b.equals(Button.LEFT)) { 
            // decrease the offset
            offset--;
        } else if (b.equals(Button.RIGHT)) { 
            // decrease the offset
            offset++;
        }
 
        LCD.drawInt(offset, 8, 0);
    }
}
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License