% Differential drive robot: heading control 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
% through a point-to-point path.
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)
% CASE 3
K_p = 0.4; % Proportional gain coefficient
K_d = -0.02; % Derivative gain coefficient
% Calculate random points to go to
N = 5;
points = 1*rand(N,2);
desiredTargetPoint = points(1,:);

% 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.
dt = 0.2;
t_end = 300;
t = 0;
t_array = (t:dt:t_end)';
nSteps = numel(t_array);

% Initialize results array
xs = zeros(nSteps,1);
ys = zeros(nSteps,1);
vRs = zeros(nSteps,1);
vLs = zeros(nSteps,1);
headingError_array = zeros(nSteps,1);
headingError_P_array = zeros(nSteps,1);
headingError_D_array = zeros(nSteps,1);
wLcoords = zeros(nSteps,2);
wRcoords = zeros(nSteps,2);
count = 1;
stopCycle = 0;
headingError_prev = 0; % Previous heading error (is used in calculating the derivative of the error)
for ii = 1:nSteps
    % Control
    % ---------------------------------------------------------------------
    % Here, the heading is determined by calculating the direction towards
    % the target point:
    headingVector = desiredTargetPoint - [x y];
    % We also want to determine when a point is reached. This we assume
    % when the distance between the robot and the point is less than 0.05.
    % In that case we can select the next point in the path.
    if norm(headingVector) < 0.05
        % Select new point
        count = count + 1;
        if count > N
            stopCycle = true;
            break
        end
        desiredTargetPoint = points(count,:);
    end
    % Now we can calculate the heading. Note that this changes at each
    % iteration, due to the motion of the robot.
    desiredHeading = atan2(headingVector(2),headingVector(1));
    headingError = desiredHeading - theta;
    if headingError > pi
        headingError = headingError - 2*pi;
    end
    if headingError < -pi
        headingError = headingError + 2*pi;
    end
    % 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);
    dy = dq(2);
    dtheta = dq(3);
    
    % 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;
    headingError_P_array(ii) = K_p * headingError;
    headingError_D_array(ii) = K_d * derivative;
    
    % 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
    plot(points(:,1),points(:,2),':o'); % Plot the path to follow, formed by N points
    plot(desiredTargetPoint(:,1),desiredTargetPoint(:,2),'k*'); % Plot the path to follow, formed by N points
    xlim(1*[-0.3 1.2])
    ylim(1*[-0.2 1.2])
    daspect([1 1 1])
    pause(.01)
    drawnow
end

% Trim arrays
t_array = t_array(1:ii,:);
xs = xs(1:ii,:);
ys = ys(1:ii,:);
vRs = vRs(1:ii,:);
vLs = vLs(1:ii,:);
wLcoords = wLcoords(1:ii,:);
wRcoords = wRcoords(1:ii,:);
headingError_array = headingError_array(1:ii,:);
headingError_P_array = headingError_P_array(1:ii,:);
headingError_D_array = headingError_D_array(1:ii,:);

figure
subplot(3,1,1)
plot(t_array, vRs);
hold on
plot(t_array, vLs);
xlabel('Time [s]')
grid on
legend('v_R','v_L');
title('Wheel speed');

subplot(3,1,2)
plot(t_array, abs(headingError_array));
xlabel('Time [s]')
grid on
title('Error');

subplot(3,1,3)
plot(t_array, abs(headingError_P_array));
hold on
plot(t_array, abs(headingError_D_array));
xlabel('Time [s]')
grid on
legend('P term','D term');
title('Proportional/derivative errors');