% Differential drive robot: forward kinematics
% =========================================================================
% In this exercise we can see how a simple differential drive robot behaves
% based on the speed of its wheels.
clc
% cla
clear

% Parameters
% -------------------------------------------------------------------------
v_R = -1.0;
v_L = 1.0;
wr = 0.1; % Wheel radius
d = 0.3;

% Initial conditions
% -------------------------------------------------------------------------
% < x , y > defines the position of the robot in the global inertial frame
% of reference < X_I , Y_I >.
% The orientation of the robot is defined by the angle theta with respect
% to the x axis of the global inertial frame of reference.
x = 0;
y = 0;
theta = pi/3;
% In this case the robot is initially placed at an angle of pi/3.

% Setup time integration
% -------------------------------------------------------------------------
% Here are defined the parameters which concern the time-integrated
% simulation, namely the initial time t, the timestep dt and the end time
% t_end.
t = 0;
dt = 0.05;
t_end = 4;

% Create figure
% -------------------------------------------------------------------------
hold on
xlabel('$X_I [m]$','Interpreter','latex')
ylabel('$Y_I [m]$','Interpreter','latex')
set(gcf,'WindowStyle','docked')

% Simulate
% -------------------------------------------------------------------------
ii = 0;
while t <= t_end
    
%     v_L = v_L + 0.05
    
    % Forward kinematics model
    d_xi_R = [v_R/2 + v_L/2          ; ...
              0                      ; ...
              v_R/(d) - v_L/(d) ];
          
    R_inv = [cos(theta) -sin(theta) 0 ; ...
             sin(theta)  cos(theta) 0 ; ...
             0           0          1 ];
         
    d_xi_I = R_inv * d_xi_R;
    
    % Integration of speed and position
    x = x + d_xi_I(1)*dt;
    y = y + d_xi_I(2)*dt;
    theta = theta + d_xi_I(3)*dt;
    
    ii = ii + 1;
    xi_array(ii,:) = [x, y, theta]';
    
    % Coordinates of wheels (display purposes only)
    x1l = x - d/2 * cos(theta+pi/2) + wr * cos(theta);
    y1l = y - d/2 * sin(theta+pi/2) + wr * sin(theta);
    x2l = x - d/2 * cos(theta+pi/2) - wr * cos(theta);
    y2l = y - d/2 * sin(theta+pi/2) - wr * sin(theta);
    x1r = x + d/2 * cos(theta+pi/2) + wr * cos(theta);
    y1r = y + d/2 * sin(theta+pi/2) + wr * sin(theta);
    x2r = x + d/2 * cos(theta+pi/2) - wr * cos(theta);
    y2r = y + d/2 * sin(theta+pi/2) - wr * sin(theta);
    
    % Plot
    clf
    plot([x x+0.2*cos(theta)],[y y+0.2*sin(theta)],'b')
    hold on
    plot(x,y,'bo')
    plot([x1l x2l],[y1l y2l], 'lineWidth', 2,'color','k'); % Right wheel
    plot([x1r x2r],[y1r y2r], 'lineWidth', 2,'color','r'); % Left wheel
%     xlim([-2.5 2.5])
%     ylim([-0.5 4.0])
    xlim([-1 1]);
    ylim([-0.5 1]);
    daspect([1 1 1])
    drawnow
    pause(.01)
    
    % Increment time
    t = t + dt;
end


% figure
plot(xi_array(:,1),xi_array(:,2));