|     ---------                    ---- I_limit -----------
|    /      ki_1  \               /
|  / ki                \         / ki
|/                        \     /                     ki < ki_1
|                            \ /
|-----------
|                \
|                  \
|                    \
|                      \  d_err<0
|                        \
|                          \
|                            \--------------------error
|
|____________________________________________

|                   / -
|                 /      \
|                /           \
|              /               \
|             /                  \
|---------         < -TM+LAG->     \-------- D
|
|
|            --------------------- error
|           / 
|-----------/
|__________________________________________

#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Motor, motorB, , tmotorNormal, PIDControl)
#pragma config(Motor, motorC, , tmotorNormal, PIDControl)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

float Time=0;
float Sum_err = 0;
float Sum_err_End = 0;
float err = 0;
float d_err = 0; //error derivative
float last_err=0;
float d_val_0= 0;
float d_val_1= 0;
float P = 0;
float I = 0;
float D = 0;
float pid = 0;
float I_old = 0;
float rest_I = 0;
float delta_I=0;

const int sp_hist=20; //set point hystersa
const float Sample_T=10; //sampling time, also inflences Sum_err_End used for efficiency estimation

bool test = false;
bool test_passed = false;
int sp = 500; //set point for error calculation

float TM_LAG=1.0; // [sec] variables for impulse height and witdh definition
float sRueck=0;
float sRestDiff=0;
float D_anteil=0;
float RueckDiff=0;
float RueckAlt=0;

const int pid_limit = 60;
const float kp = 0.85; //1.5
const float ki = 0.12; // in [sec], integral part of pid
const float ki_1 = 0.12; // in [sec], kind od antywindup: if I reaches i_limit & error decreases, I action is dynamicaly reduced, ki_1 is the integral time in [sec] for reduction
const float kd = 0.3; // derivative
int BC_power = 28; // motors' power, for original Lego test pad (simple black cirle) BC_power might be 50-70%
const float BC_const = 1; // reduce the opposite wheel's power for BC_const fator to make the curves smoother
const int i_limit = 60; // integral limit
const bool block = false; // for tests only
//int i=1;
//+ PID - PID
// RESULTS
// BC_power BC_const kp ki ki_1 kd Sum_Err Time Sample_T
// 40 0.15 1.3 1.9 0.9 0.3 130 8.9 10 ms
// 40 0.1 1.5 1.1 0.3 0.07 135 8.5


void PID_Calc()
{
P=kp*err; // Propotional action

if (kd!=0.0) // Differential action, it calculates impulse height and width depending on TM_LAG
{
if (TM_LAG < (Sample_T/2)/1000) TM_LAG=(Sample_T/2)/1000;
if ((sRueck<-1000.0)||(sRueck<-1000.0)) sRueck=0.0;
if ((sRestDiff<-1000.0)||(sRestDiff>1000.0)) sRestDiff=0.0;
D_anteil=(P-sRueck)*kd/((Sample_T/1000*0.5)+TM_LAG);
RueckAlt=sRueck;
RueckDiff=(Sample_T/1000)/kd * D_anteil + sRestDiff;
sRueck=RueckDiff+RueckAlt;
sRestDiff=RueckAlt-sRueck+RueckDiff;
D=D_anteil;
};

if (ki!=0) // integral action
{
I_old = I;
if ((d_err>=0) & (abs(err)>7))
delta_I=(kp*(err+last_err)*0.5*(Sample_T/1000)/abs(ki));// + rest_I;
else
delta_I=-(kp*(err+last_err)*0.5*(Sample_T/1000)/abs(ki_1));// + rest_I;
if ((delta_I<0.000001)&(delta_I>-0.000001)) delta_I=0.0;
I = I_old + delta_I; //trapesoid integration
if (I > i_limit) {I = i_limit;I_old=i_limit;};
if (I < -i_limit) {I = -i_limit;I_old=-i_limit;};
}

pid = abs(P+I+D);
last_err = err;
if (pid > pid_limit) pid = pid_limit;
}

task d_light_PID() // start another task just not to interupt the main tastk by wait1Msec()-s
{
while (true)
{
for (int j=1; j<10;j++) // calculate an average Ligh val for dif err estiamtion, kinf of filter
{
wait1Msec(Sample_T/10);
d_val_0 += SensorRaw[light];
}
d_val_0 = d_val_0/10;
wait1Msec(Sample_T);
d_val_1 = SensorRaw[light];
err = (d_val_1 - sp)/10;
d_err = (d_val_1 - d_val_0)/(Sample_T/1000);
if ((0<abs(d_err))&(abs(d_err)<=100)) d_err=0;
PID_Calc();
// This is just for PID efficiency estimation after line run,
// the closer the line the smaller Sum_err value should be.
Sum_err += abs(err)/10;
}
}

//1. Test whether NXT is on the left or on the right side of the line.
//2. Find the min & max value to calculate Set Point for PID
//3. If after back motor move, sensor finds lower than Min or higher than Max values,
// min or max values are corrected respectively. This helps better SP estimation.

void Test_Line_Side_and_SP()
{
int min_val=0;
int max_val=0;
int old_val=0;

old_val = SensorRaw[light];
motor[motorB]=15;
//motor[motorC]=-15;
wait1Msec(400);
motor[motorB]=0;
if (SensorRaw[light] < old_val)
{
min_val = SensorRaw[light];
max_val=old_val;
}
else
{
max_val = SensorRaw[light];
min_val = old_val;
}
motor[motorB]=-15;
wait1Msec(400);
motor[motorB]=0;
motor[motorC]=0;
if (SensorRaw[light] < min_val) min_val = SensorRaw[light];
if (SensorRaw[light] > max_val) max_val = SensorRaw[light];
wait1Msec(600);
if ((old_val - SensorRaw[light])>0) test = true;
else test = false;
test_passed = true;
//sp = (min_val + max_val)/2;
ClearTimer(T1);
}

task main ()
{
wait1Msec(100);
StartTask(d_light_PID);

while (true)
{
if (!test_passed)Test_Line_Side_and_SP();
if (block==false)
{
//if ((d_err>5900) & (Time>23000)) // may be used to stop with Time & Sum_err_End measurment
//{ // at the end of cirle, when white tape (marker) is sticked across the black one
//motor[motorB]=0;
//motor[motorC]=0;
//err=0;
//Sum_err_End = Sum_err;
//break;
//}
if ((sp-sp_hist)<SensorRaw[light] & SensorRaw[light]<(sp+sp_hist))
{
I = 0; // if the robot is "on the line", "cancel" the pid,
err = 0;
motor[motorB]=BC_power+5;
motor[motorC]=BC_power+5;
}
else
{
if (!test)
{
if (err>0)
{
motor[motorB]=BC_power + pid;
motor[motorC]=BC_power - BC_const*pid;
}
else
{
motor[motorB]=BC_power - BC_const*pid;
motor[motorC]=BC_power + pid;
}
}
else
{
if (err>0)
{
motor[motorC]=BC_power + pid;
motor[motorB]=BC_power - BC_const*pid;
}
else
{
motor[motorC]=BC_power - BC_const*pid;
motor[motorB]=BC_power + pid;;
}
}
}
}
Time=time1(T1);
Sum_err_End = Sum_err;
}
}