Controlling Mobile Robots Week 3 Programming Assignment GoToGoal

1-quickbot.md

GoToGoal Controller

This project implements a GoToGoal controller for the QuickBot differential drive robot.

A screenshot of the robot successfully turning and driving towards the goal coordinate can be seen below in the first image. As seen in the recording, the robot is able to quickly and smoothly turn towards the goal position. This is controlled by the value selected for the PID gain factor obj.Kp = 2 in the GoToGoal.m constructor.

If the selected value for the PID gain is too small or large, the robot may be unable to reach the goal.

Too Large PID Gain

If the PID gain is too large, the robot will fluctuate and wiggle rapidly as it moves towards to goal in a non-optimal fashion. See the second screenshot.

Too Small PID Gain

If the PID gain is too small, the robot may completely miss the target goal as it turns too slowly, eventually resulting in a collision with the wall. See the third screenshot.

QBSupervisor.m Constructor

obj.theta_d     = pi/4;
obj.v           = 0.2;
obj.goal        = [1, -1];
obj.d_stop      = 0.05;

GoToGoal.m Constructor

% initialize memory banks
obj.Kp = 2;
obj.Ki = 0;
obj.Kd = 0;

GoToGoal.m

function outputs = execute(obj, robot, state_estimate, inputs, dt)
%% EXECUTE Computes the left and right wheel speeds for go-to-goal.
%   [v, w] = execute(obj, robot, x_g, y_g, v) will compute the
%   necessary linear and angular speeds that will steer the robot
%   to the goal location (x_g, y_g) with a constant linear velocity
%   of v.
%
%   See also controller/execute

    % Retrieve the (relative) goal location
    x_g = inputs.x_g; 
    y_g = inputs.y_g;

    % Get estimate of current pose
    [x, y, theta] = state_estimate.unpack();

    % Compute the v,w that will get you to the goal
    v = inputs.v;

    %% START CODE BLOCK %%

    % 1. Calculate the heading (angle) to the goal.

    % distance between goal and robot in x-direction
    u_x = x_g - x;     

    % distance between goal and robot in y-direction
    u_y = y_g - y;

    % angle from robot to goal. Hint: use ATAN2, u_x, u_y here.
    theta_g = atan2(u_y, u_x);

    % 2. Calculate the heading error.

    % error between the goal angle and robot's angle
    % Hint: Use ATAN2 to make sure this stays in [-pi,pi].
    e = theta_g - theta;
    e_k = atan2(sin(e), cos(e));

    % 3. Calculate PID for the steering angle 

    % error for the proportional term
    e_P = e_k;

    % error for the integral term. Hint: Approximate the integral using
    % the accumulated error, obj.E_k, and the error for
    % this time step, e_k.
    e_I = e_k + obj.E_k;

    % error for the derivative term. Hint: Approximate the derivative
    % using the previous error, obj.e_k_1, and the
    % error for this time step, e_k.
    e_D = e_k - obj.e_k_1;

    %% END CODE BLOCK %%

    w = obj.Kp*e_P + obj.Ki*e_I + obj.Kd*e_D;

    % 4. Save errors for next time step
    obj.E_k = e_I;
    obj.e_k_1 = e_k;

    % plot
    obj.p.plot_2d_ref(dt, atan2(sin(theta),cos(theta)), theta_g, 'r');

    outputs = obj.outputs;  % make a copy of the output struct
    outputs.v = v;
    outputs.w = w;
end

QBSupervisor.m

function [vel_r, vel_l] = ensure_w(obj, robot, v, w)

    % This function ensures that w is respected as best as possible
    % by sacrificing v.

    % 1. Limit v,w from controller to +/- of their max
    w = max(min(w, obj.w_max_v0), -obj.w_max_v0);
    v = max(min(v, obj.v_max_w0), -obj.v_max_w0);

    % 2. Compute desired vel_r, vel_l needed to ensure w
    [vel_r_d, vel_l_d] = obj.robot.dynamics.uni_to_diff(v,w);

    % 3. Find the max and min vel_r/vel_l
    vel_rl_max = max(vel_r_d, vel_l_d);
    vel_rl_min = min(vel_r_d, vel_l_d);

    % 4. Shift vel_r and vel_l if they exceed max/min vel

    %% START CODE BLOCK %%

    if (vel_rl_max > obj.robot.max_vel)
        vel_r = vel_r_d - (vel_rl_max - obj.robot.max_vel);
        vel_l = vel_l_d - (vel_rl_max - obj.robot.max_vel);
    elseif (vel_rl_min < -obj.robot.max_vel)
        vel_r = vel_r_d - (vel_rl_min + obj.robot.max_vel);
        vel_l = vel_l_d - (vel_rl_min + obj.robot.max_vel);
    else
        vel_r = vel_r_d;
        vel_l = vel_l_d;
    end

    %% END CODE BLOCK %%

    % 5. Limit to hardware
    [vel_r, vel_l] = obj.robot.limit_speeds(vel_r, vel_l);
end

z-1-quickbot.gif

z-2-quickbot-too-large-pid-gain.gif

z-3-quickbot-too-small-pid-gain.gif