% Differential drive robot: target heading through P controller
% =========================================================================
% In this exercise we can see how a differential drive robot can be
% controlled in its heading with a Proportional controller to travel
% towards a target point.
clc
clear all
% close all

% Parameters
% -------------------------------------------------------------------------
% We define the main parameters for the problem
v = 0.05; % Forward velocity of the robot (constant)
d = 0.2; % Wheelbase (distance between left and right wheels)
wheel_radius = 0.05; % Radius of the wheels (only used for display purposes)
desiredTargetPoint = [1; 0]; % Target point that we want our robot to reach
% CASE 1
% K_p = 0.1; % Proportional gain coefficient
% K_d = 0.0; % Derivative gain coefficient
% CASE 2
% K_p = 0.4; % Proportional gain coefficient
% K_d = 0.0; % Derivative gain coefficient
% CASE 3
K_p = 0.1; % Proportional gain coefficient
K_d = -0.1; % Derivative gain coefficient

% 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_I axis of the global inertial frame of reference.
theta = 150 *pi/180;
x = 0;
y = 0;

% 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.1;
t_end = 30;

% Initialize results array
% -------------------------------------------------------------------------
t_array = t:dt:t_end;
nSteps = numel(t_array);
xs = zeros(nSteps,1);
ys = zeros(nSteps,1);
vRs = zeros(nSteps,1);
vLs = zeros(nSteps,1);
headingError_array = zeros(nSteps,1);
wLcoords = zeros(nSteps,2);
wRcoords = zeros(nSteps,2);
headingError_prev = 0; % Previous heading error (is used in calculating the derivative of the error)
for ii = 1:nSteps
    if theta < - pi
        theta = theta + 2*pi;
    end
    if theta > + pi
        theta = theta - 2*pi;
    end
    % Control
    % ---------------------------------------------------------------------
    % Here, the heading is determined by calculating the direction towards
    % the target point:
    headingVector = desiredTargetPoint - [x;y];
    desiredHeading = atan2(headingVector(2),headingVector(1));
    headingError = desiredHeading - theta;
    % We set the derivative to 0 in the first step of computation
    if ii == 1
        derivative = 0;
    else
        derivative = (headingError - headingError_prev)/dt;
    end
    % Now we apply the control to the controlled variables v_R and v_L:
    v_R = v + (K_p * headingError + K_d * derivative);
    v_L = v - (K_p * headingError + K_d * derivative);
    % We record the current heading error:
    headingError_prev = headingError;
    
    % Kinematics model
    % ---------------------------------------------------------------------
    dq = [1/2*cos(theta) ; 1/2*sin(theta) ;  1/(2*d)] * v_R + ...
         [1/2*cos(theta) ; 1/2*sin(theta) ; -1/(2*d)] * v_L;
    dx = dq(1); % Velocity along X_I
    dy = dq(2); % Velocity along Y_I
    dtheta = dq(3); % Rotation rate of the robot
    
    % Time integration
    % ---------------------------------------------------------------------
    x = x + dx*dt;
    y = y + dy*dt;
    theta = theta + dtheta*dt;
    
    % Calculate position of wheels
    % ---------------------------------------------------------------------
    xwL = x + d*cos(theta+pi/2);
    ywL = y + d*sin(theta+pi/2);
    xwR = x + d*cos(theta-pi/2);
    ywR = y + d*sin(theta-pi/2);
    % Coordinates of wheels (display purposes only)
    x1l = x - d * cos(theta+pi/2) + wheel_radius * cos(theta); % Left wheel tip x-coordinate (1)
    y1l = y - d * sin(theta+pi/2) + wheel_radius * sin(theta); % Left wheel tip y-coordinate (1)
    x2l = x - d * cos(theta+pi/2) - wheel_radius * cos(theta);
    y2l = y - d * sin(theta+pi/2) - wheel_radius * sin(theta);
    x1r = x + d * cos(theta+pi/2) + wheel_radius * cos(theta);
    y1r = y + d * sin(theta+pi/2) + wheel_radius * sin(theta);
    x2r = x + d * cos(theta+pi/2) - wheel_radius * cos(theta);
    y2r = y + d * sin(theta+pi/2) - wheel_radius * sin(theta);
    
    % Save in array
    % ---------------------------------------------------------------------
    xs(ii) = x;
    ys(ii) = y;
    vRs(ii) = v_R;
    vLs(ii) = v_L;
    wLcoords(ii,:) = [xwL ywL];
    wRcoords(ii,:) = [xwR ywR];
    headingError_array(ii) = headingError;
    
    % Plot
    % ---------------------------------------------------------------------
    cla
    x2 = x + d*cos(theta); % "fictional" front of the robot
    y2 = y + d*sin(theta);
    plot(xs(1:ii),ys(1:ii),'color',[0.8500 0.3250 0.0980]); % Plots the robot trajectory
    hold on;
    plot([x desiredTargetPoint(1)],[y desiredTargetPoint(2)],':') % Plots the desired target point
    plot(wLcoords(1:ii,1),wLcoords(1:ii,2),'color',[0.9290 0.6940 0.1250]); % Plot the left wheel trajectory
    plot(wRcoords(1:ii,1),wRcoords(1:ii,2),'color',[0.9290 0.6940 0.1250]); % Plot the right wheel trajectory
    plot([x x2],[y y2],'lineWidth',2,'color',[0 0.4470 0.7410]); % Plots the robot body
    plot([xwR xwL],[ywR ywL],'lineWidth',2,'color',[0 0.4470 0.7410]); % Plots the wheel axis
    plot([x1l x2l],[y1l y2l], 'lineWidth', 2,'color','k'); % Rear wheel
    plot([x1r x2r],[y1r y2r], 'lineWidth', 2,'color','r'); % Front wheel
    xlim([-0.3 1.2])
    ylim([-0.2 0.5])
    daspect([1 1 1])
    pause(.01)
    drawnow
end

figure
plot(t_array, vRs);
hold on
plot(t_array, vLs);
xlabel('Time [s]')
grid on
legend('v_R','v_L');

figure
plot(t_array, abs(headingError_array));
xlabel('Time [s]')
grid on
title('Error');