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

function value=errorPIDgaPointShoot2(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.02;

%%the right wheel velocity
vr=0.02;
%%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=10;
timestep=0.1;

%% 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=[];
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);
    
    ct=Pe*dyt+Pi*yt+Pd*d2yt;
    %%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(Yd);
end