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


%% This script initialize the simulation and uses
%% the kinematics formula with a Taylor approssimation
%% that is valid only when the robot follow a straight line
%% in this case when the robot follow the x axis


%% vl,vr expressed in meters/seconds
%%the left wheel velocity
vl=0.018
%%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=0
tend=4
timestep=0.1

%%now define the number of time steps
nsteps=(tend-tstart)/timestep;
%%this is the starting position for the robot
start_pos=[0,0,theta0];
position_pts2=[start_pos];
old_pos=start_pos;
lwheel_trace2=LeftWheelLoc(start_pos,b);
rwheel_trace2=RightWheelLoc(start_pos,b);
figure(1);
title('Differential steering robot with exact kinematics');
DrawRobotPosition(old_pos,b);
for t=tstart:timestep:tend
new_pos=deadReckonTimeVelocity2(old_pos,vr,vl,t,b);
position_pts2=[position_pts2; new_pos];
lwheel_trace2=[lwheel_trace2; LeftWheelLoc(new_pos,b)];
rwheel_trace2=[rwheel_trace2; RightWheelLoc(new_pos,b)];
old_pos=new_pos;
DrawRobotPosition(old_pos,b);
end

position_pts1=[start_pos];
old_pos=start_pos;
lwheel_trace1=LeftWheelLoc(start_pos,b);
rwheel_trace1=RightWheelLoc(start_pos,b);
figure(2);
title('Differential steering robot simulation with Taylor approssimation');
DrawRobotPosition(old_pos,b);
for t=tstart:timestep:tend
new_pos=deadReckonTimeVelocityTaylor(old_pos,vr,vl,t,b);
position_pts1=[position_pts1; new_pos];
lwheel_trace1=[lwheel_trace1; LeftWheelLoc(new_pos,b)];
rwheel_trace1=[rwheel_trace1; RightWheelLoc(new_pos,b)];
old_pos=new_pos;
DrawRobotPosition(old_pos,b);
end

%% Draw the traces for the left and right wheel
figure(3)
title('Wheel path comparison');
DrawWheelTrace(lwheel_trace1,'r');
DrawWheelTrace(rwheel_trace1,'r');

DrawWheelTrace(lwheel_trace2,'b');
DrawWheelTrace(rwheel_trace2,'b');


%%Calculate the error between the 2 methods
err=norm(position_pts1-position_pts2);
legend('lwheel trace exact','rwheel trace exact',...
'lwheel trace approx','rwheel trace approx');
xlabel(['The error is ',num2str(err)]);
figure(1);
%%now define the space boundaries
%%becareful to plan  the traj$ectory well or the robot will
%%go outside
xmin=min(position_pts1(:,1))-b;
xmax=max(position_pts1(:,1))+b;
ymin=min(position_pts1(:,2))-b;
ymax=max(position_pts1(:,2))+b;
box_space=[xmin xmax ymin ymax];
axis(box_space);

xlabel('X coordinate');
ylabel('Y coordinate');

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


xlabel('X coordinate');
ylabel('Y coordinate');

%%Compare the trajectories between exact and taylor approssimation
figure(4);
grid on;
hold on;
title('Coordinates comparison');
plot(position_pts1(:,1),'r-');
plot(position_pts1(:,2),'r--');
plot(position_pts2(:,1),'b-');
plot(position_pts2(:,2),'b--');

legend('x','y','x taylor','y taylor');

%%Calculate the drift and its rate both with Taylor