%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL


%% This script initialize the simulation and uses
%% PID controller with the exact kinematics
function [Yd,Y2d,Ct,position_pts]=simPIDWindup(K,decayInt,scale,time,draw)
close all
%% vl,vr expressed in meters/seconds
%%the left wheel velocity
vl=0.02;
%%the maximum left speed 
vmax=0.08;
%%the right wheel velocity
vr=0.02;

%%the initial orientation of the robot
%%the angle is counter clockwise from the x axis
theta0=0;
%%b is the axis length of the robot that connect the 2 wheels expressed in
%%meters
b=0.5;
%%the run time
tstart=time(1);
tend=time(2);
timestep=0.1;

%% The PID parameters
Pe=K(1);
Pi=K(2);
Pd=K(3);


%%now define the number of time steps
nsteps=(tend-tstart)/timestep;
%%this is the starting position for the robot
start_pos=[0,0.05,theta0];
position_pts=[start_pos];
old_pos=start_pos;
lwheel_trace=LeftWheelLoc(start_pos,b);
rwheel_trace=RightWheelLoc(start_pos,b);
%%the correction control
Ct=[];
%% the drift y derivative
Yd=[];
%% the drift rate y 2th derivative
Y2d=[];

%%
if(draw==1)
figure(1);
DrawRobotPosition(old_pos,b);
end
for t=tstart:timestep:tend
    new_pos=deadReckonTimeVelocity(old_pos,vr,vl,t,b);
    position_pts=[position_pts; new_pos];
    lwheel_trace=[lwheel_trace; LeftWheelLoc(new_pos,b)];
    rwheel_trace=[rwheel_trace; RightWheelLoc(new_pos,b)];
    old_pos=new_pos;
    %%compute the PID controller with the Taylor approssimator
    %%this is the error function
    errorInt=dydt(vr,vl,theta0,t-tstart,b);
    errorDiff=d2ydt2(vr,vl,theta0,t-tstart,b);
    
    Yd=[Yd;errorInt];
    Y2d=[Y2d;errorDiff];
    %%yt is the integral error
    error=new_pos(2);
    errorInt=errorInt+error-errorInt*decayInt/100;
    
    ct=(Pe*errorInt+Pi*error+Pd*errorDiff)/scale;
    Ct=[Ct;ct];
    if(1)

        %%use the error control on the steering system
        if(ct>0)
            if(ct>vmax)
                ct=vmax;
            end
        else
            if(ct<-vmax)
                ct=(-vmax);
            end
        end
        vr=vmax/2-ct/2;
        vl=vmax/2+ct/2;
    end
    if(draw==1)
    DrawRobotPosition(old_pos,b);
    end
  %%  pause(0.2);
end

%% Draw the traces for the left and right wheel
if(draw==1)
figure(2)
DrawWheelTrace(lwheel_trace,'b');
DrawWheelTrace(rwheel_trace,'b');

%%now define the space boundaries
%%becareful to plan  the trajectory well or the robot will
%%go outside
figure(1)
xmin=min(position_pts(:,1))-b
xmax=max(position_pts(:,1))+b
ymin=min(position_pts(:,2))-b
ymax=max(position_pts(:,2))+b
box_space=[xmin xmax ymin ymax];
axis(box_space);

title('Differential steering robot simulation');
xlabel('X coordinate');
ylabel('Y coordinate');
figure(3);
title('Drift term and drift rate')
hold on;
grid on;
plot(Yd,'r--');
plot(Y2d,'b--');
plot(Ct,'g--');
legend('Integral error','Diff error','Control error');
xlabel('Time');
ylabel('y');
hold off;

figure(4);
title('Trajectories');
grid on;
hold on;
plot(position_pts(:,1),'r--');
plot(position_pts(:,2),'b--');
legend('x','y');
end
end