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

function value=errorPIDgaNoWind4(K)
%% This script initialize the simulation and uses
%% PID controller with the exact kinematics

%% vl,vr expressed in meters/seconds
%%the left wheel velocity
vl=0.06;

%%the right wheel velocity
vr=0.06;
%%the maximum right speed
vmax=0.1;
%%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=0;
tend=8;
timestep=0.05;

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

%%this is the starting position for the robot
start_pos=[0,0.05,theta0];
%% the drift y derivative
Yd=[];
%% the integral term y
Y=[];
Ydd=[];
%%After some second we want the robot be stabilized!
Ymask1=[0 0];
Ymask2=[];
old_pos=start_pos;

for t=tstart:timestep:tend
    new_pos=deadReckonTimeVelocity3(old_pos,vr,vl,t,b);
    old_pos=new_pos;
    %%compute the PID controller with the Taylor approssimator
    dyt=dydt(vr,vl,theta0,t-tstart,b);
    d2yt=d2ydt2(vr,vl,theta0,t-tstart,b);
    Yd=[Yd;dyt];
    yt=new_pos(2);
    Y=[Y; yt];
    Ydd=[Ydd;d2yt];
    ct=Pe*dyt+Pi*yt+Pd*d2yt;
    
    if(t>6)
        Ymask1=[Ymask1; yt d2yt];
    else
        Ymask2=[Ymask2; yt];
    end
    %%use the error control on the steering system
        %%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
value=norm(Ymask1(:,1));
end