import lejos.nxt.*;

/**
 *   adapted from:
 *   http://legolab.cs.au.dk/DigitalControl.dir/NXT/Lesson5.dir/Lesson5Programs/Sejway.java
 *
 *   PID control loop - proportional, integral, derivative
 *   *   proportional - measures the current error
 *       and determines how much it should adjust to
 *       achieve balance
 *
 *   *   integral - determines the amount of time that
 *       the error went uncorrected
 *
 *   *   derivative - tries to anticipate future error
 *       from the rate of change over time
 *
 *   @author Brian Bagnall
 *   @author adapted by Scott Burgess
 *   @author adapted by Sharon Tuttle
 *   @version 2015-04-15
 */

public class Sejway 
{
    // PID constants

    private static final int KP = 28;
    private static final int KI = 4;
    private static final int KD = 33;
    private static final int SCALE = 18;

    // additional data fields

    private int offset;
    private int prevError;
    private float intError;
    
    private LightSensor lightSens;
    
    /** 
     *   constructor
     */

    public Sejway() 
    {
        this.lightSens = new LightSensor(SensorPort.S2, true);

        this.offset = 0;
        this.prevError = 0;
        this.intError = 0;
    }
    
    /**
     *  does this allow user to essentially calibrate
     *  a balanced position?
     */

    public void getBalancePos() 
    {
        // determine what "balanced" means, for the light sensor;
        // wait for user to balance and then press orange/enter button

        while (!Button.ENTER.isDown())
        {
            // NXTway must be balanced.

            this.offset = lightSens.readNormalizedValue();
            LCD.clear();
            LCD.drawInt(this.offset, 2, 4);
            LCD.refresh();
        }
    }
    
    /**
     *  the PID control loop -- to try to correct imbalance
     *  with less potential for wide fluctuation
     */

    public void pidControl() 
    {
        while (!Button.ESCAPE.isDown())
        {
            int normVal = lightSens.readNormalizedValue();

            // Proportional Error:

            int error = normVal - this.offset;

            // Adjust far and near light readings:

            if (error < 0) 
            {
                error = (int)(error * 1.8F);
            }
            
            // Integral Error:

            this.intError = ((this.intError + error) * 2)/3;
            
            // Derivative Error:

            int derivError = error - this.prevError;
            this.prevError = error;
            
            int pidVal = (int)(KP * error + 
                               KI * intError + 
                               KD * derivError) / SCALE;
            
            if (pidVal > 100)
            {
                pidVal = 100;
            }

            if (pidVal < -100)
            {
                pidVal = -100;
            }

            // Power derived from PID value:

            int power = Math.abs(pidVal);
            power = 55 + (power * 45) / 100; // NORMALIZE POWER

            if (pidVal > 0) 
            {
                MotorPort.B.controlMotor(power, BasicMotorPort.FORWARD);
                MotorPort.C.controlMotor(power, BasicMotorPort.FORWARD);
            } 
            else 
            {
                MotorPort.B.controlMotor(power, BasicMotorPort.BACKWARD);
                MotorPort.C.controlMotor(power, BasicMotorPort.BACKWARD);
            }
        }
    }
    
    /**
     *  shut down light sensor and motors
     */

    public void shutDown() 
    {
        // Shut down light sensor, motors

        Motor.B.flt();
        Motor.C.flt();
        lightSens.setFloodlight(false);
    }

    /**
     *  set up and start the Sejway robot
     *
     *  @param args not used
     */
    
    public static void main(String[] args) 
    {
        Sejway mySejway = new Sejway();
        mySejway.getBalancePos();
        mySejway.pidControl();
        mySejway.shutDown();
    }
}