% Bicycle drive robot: heading control through PD controller
% =========================================================================
% In this exercise we can see how a steered bicycle robot can be controlled
% in its heading with a Proportional-Derivative controller
clc
clear all
close all

% Parameters
% -------------------------------------------------------------------------
% We define the main parameters for the problem
v = 0.05; % Forward velocity of the robot (constant)
l = 0.2; % Wheelbase (distance between rear and front wheel)
wheel_radius = 0.05; % Radius of the wheels (only used for display purposes)
desiredHeading = 0; % Heading setpoint (angle psi* in the slides)
K_p = 0.7; % Proportional gain coefficient
K_d = 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.
% The angle rho determines the angular coordinate of the steering axis,
% while drho is its rotation speed.
theta = 20 *pi/180; % Initial orientation of the robot with respect to < X_I , Y_I >
rho = -10 *pi/180; % Initial steering angle
drho = 0; % Initial steering angle rotation speed
x = 0; % Initial position along the inertial X_I axis
y = 0; % Initial position along the inertial Y_I axis
% In this case the robot is initially placed at <0,0> at an angle of 20
% degrees, the steering axis is turned to the right by 10 degrees, and its
% speed is 0 rad/s.

% 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 = 34;

% Initialize results array
% -------------------------------------------------------------------------
t_array = t:dt:t_end;
nSteps = numel(t_array);
xs = zeros(nSteps,1);
ys = zeros(nSteps,1);
headingError_array = zeros(nSteps,1);
headingError_prev = 0; % Previous heading error (is used in calculating the derivative of the error)
for ii = 1:nSteps
    % Fix angles
    % ---------------------------------------------------------------------
    % This is to avoid errors in the computations when angles > pi or < -pi
    if theta < - pi
        theta = theta + 2*pi;
    end
    
    % Control
    % ---------------------------------------------------------------------
    % Here we implement the controller. First we calculate the heading
    % error, called epsilon in the slides:
    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
    % We compute the value of the controlled variable, d_rho, the
    % rotational speed of the steering axis:
    drho = K_p * headingError + K_d * derivative;
    % We record the current heading error
    headingError_prev = headingError;
    
    % Kinematics model
    % ---------------------------------------------------------------------
    dq = [cos(theta) ;...
          sin(theta) ;...
          tan(rho)/l ;...
          0           ] * v + [0;0;0;1] * drho;
    dx = dq(1); % Velocity along X_I
    dy = dq(2); % Velocity along Y_I
    dtheta = dq(3); % Rotation rate of the robot
    drho = dq(4); % Steering angle rotation rate
    
    % Time integration
    % ---------------------------------------------------------------------
    x = x + dx*dt;
    y = y + dy*dt;
    theta = theta + dtheta*dt;
    rho = rho + drho*dt;
    
    % Calculate other points
    % ---------------------------------------------------------------------
    x2 = x + l*cos(theta); % Steering axis x-coordinate
    y2 = y + l*sin(theta); % Steering axis y-coordinate
    x3r = x - wheel_radius*cos(theta); % Rear wheel tip x-coordinate (1)
    y3r = y - wheel_radius*sin(theta); % Rear wheel tip y-coordinate (1)
    x4r = x + wheel_radius*cos(theta); % Rear wheel tip x-coordinate (2)
    y4r = y + wheel_radius*sin(theta); % Rear wheel tip y-coordinate (2)
    x3f = x2 - wheel_radius*cos(theta + rho); % Front wheel tip x-coordinate (1)
    y3f = y2 - wheel_radius*sin(theta + rho); % Front wheel tip y-coordinate (1)
    x4f = x2 + wheel_radius*cos(theta + rho); % Front wheel tip x-coordinate (2)
    y4f = y2 + wheel_radius*sin(theta + rho); % Front wheel tip y-coordinate (2)
    
    % Save in array
    % ---------------------------------------------------------------------
    xs(ii) = x;
    ys(ii) = y;
    x2s(ii) = x2;
    y2s(ii) = y2;
    headingError_array(ii) = headingError;
    
    % Plot
    % ---------------------------------------------------------------------
    cla
    plot(xs(1:ii),ys(1:ii));
    hold on;
    plot(x2s(1:ii),y2s(1:ii));
    plot([x x2],[y y2],'linewidth',2);
    plot([x3r x4r],[y3r y4r], 'lineWidth', 2,'color','k'); % Rear wheel
    plot([x3f x4f],[y3f y4f], 'lineWidth', 2,'color','k'); % Front wheel
    plot(x4f,y4f, '.', 'markersize', 14,'color','r'); % Front wheel marker
    xlim([-0.2 2.0])
    ylim([-0.2 0.4])
    daspect([1 1 1])
    drawnow
end

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