/* File: lineFollowing_PID.ch
   The NXT robot will follow a line based on PID control theory */
#include <mindstorms.h>
#include <math.h>

/* flags for determining whether the while-loop is running at the first time */
#define FALSE 0
#define TRUE  1

CMindstorms robot;

double offset;  // how much to subtract from a raw light reading to convert it to an error value
double black = 33,  // light reading for color black
       yellow = 71;  // light reading for color yellow
double Kp = 0.42,  // proportional constant
       Ki = 0.75,  // integral constant
       Kd = 0.01;  // derivative constant
double targetPower = 10;  // the power level of both motors when the error is 0 and the robot goes straight ahead
double Error,  // it tells us how far off the line's edge we are
       lastError;
double Integral = 0,
       Derivative = 0;
double turn;  // output of the PID controller
double time1,
       time2;
double dT = 1;  // the time between the last time we checked the sensor and the time of the most recent check of the sensor
       
int lightValue;
int powerLeft,  // power set for the left motor
    powerRight;  // power set for the right motor
int firstTime;  // determine whether it is the first time a while-loop has been run

robot.setSensorLight(PORT3, "Reflect"); // initialize the light sensor

offset = (yellow+black)/2;

firstTime = TRUE;

/* an infinite while-loop */
while (offset > 0) {
    robot.getSensorLight(PORT3, lightValue);
    
    robot.systemTime(time1);
    /* an if-statement to tell if it is the first time that the while-loop has been executed */
    if (firstTime == FALSE) { 
        dT = time1 - time2;
    }
    
    Error = lightValue - offset;  //proportional term in the PID controller
    Integral = (2/3)*Integral + Error*dT;  // integral term in the PID controller
    Derivative = (Error-lastError)/dT;  // derivative term in the PID controller
    turn = Kp*Error + Ki*Integral + Kd*Derivative;
    
    /* correction of the output power */
    powerLeft = round(targetPower + turn);
    powerRight = round(targetPower - turn);
    
    /* set power of two motors */
    robot.setJointPower(PORT3, powerLeft);
    robot.setJointPower(PORT2, powerRight);
    robot.moveForeverNB();
    
    lastError = Error;  // set current error to last error before we are about to get the new error when the loop cycles again
    firstTime = FALSE;  // specify when the loop cycles, it will not be the first time the loop runs
    time2 = time1;  // set current time to previous time before the loop starts all over again
}