Control of Mobile Robots, Coursera - Week 2 Programming Assignment: Differential Drive Dynamics, Robot Odometry, IR Range Sensors

1-quickbot-3.gif

2-quickbot-1.png

3-quickbot-2.png

DifferentialDrive.m

function [vel_r,vel_l] = uni_to_diff(obj,v,w)
    R = obj.wheel_radius;
    L = obj.wheel_base_length;

    %% START CODE BLOCK %%
    % Vr = (2v + wL) / 2R
    % Vl = (2v - wL) / 2R
    vel_r = ((2 * v) + (w * L)) / (2 * R);
    vel_l = ((2 * v) - (w * L)) / (2 * R);
    %% END CODE BLOCK %%
end

QBSupervisor.m

function update_odometry(obj)
%% UPDATE_ODOMETRY Approximates the location of the robot.
%   obj = obj.update_odometry(robot) should be called from the
%   execute function every iteration. The location of the robot is
%   updated based on the difference to the previous wheel encoder
%   ticks. This is only an approximation.
%
%   state_estimate is updated with the new location and the
%   measured wheel encoder tick counts are stored in prev_ticks.

    % Get wheel encoder ticks from the robot
    right_ticks = obj.robot.encoders(1).ticks;
    left_ticks = obj.robot.encoders(2).ticks;

    % Recal the previous wheel encoder ticks
    prev_right_ticks = obj.prev_ticks.right;
    prev_left_ticks = obj.prev_ticks.left;

    % Previous estimate 
    [x, y, theta] = obj.state_estimate.unpack();

    % Compute odometry here
    R = obj.robot.wheel_radius;
    L = obj.robot.wheel_base_length;
    m_per_tick = (2*pi*R)/obj.robot.encoders(1).ticks_per_rev;

    %% START CODE BLOCK %%
    N = obj.robot.encoders(1).ticks_per_rev;
    diff_ticks_l = left_ticks - prev_left_ticks;
    diff_ticks_r = right_ticks - prev_right_ticks;

    % Dl = (2 * pi * R) * (ticksL / N)
    Dl = (2*pi*R) * (diff_ticks_l / N);
    % Dr = (2 * pi * R) * (ticksR / N)
    Dr = (2*pi*R) * (diff_ticks_r / N);
    % Dc = (Dr + Dl) / 2
    Dc = (Dr + Dl) / 2;

    % x' = x + Dc * cos(theta)
    x_dt = Dc * cos(theta);
    % y' = y + Dc * sin(theta)
    y_dt = Dc * sin(theta);
    % theta' = theta + (Dr - Dl) / L
    theta_dt = (Dr - Dl) / L;

    %% END CODE BLOCK %%

    theta_new = theta + theta_dt;
    x_new = x + x_dt;
    y_new = y + y_dt;                           

    fprintf('Estimated pose (x,y,theta): (%0.3g,%0.3g,%0.3g)\n', x_new, y_new, theta_new);
    fprintf('Difference pose (x,y,theta): (%0.3g,%0.3g,%0.3g)\n', abs(x_new - x), abs(y_new - y), abs(theta_new - theta));

    % Save the wheel encoder ticks for the next estimate
    obj.prev_ticks.right = right_ticks;
    obj.prev_ticks.left = left_ticks;

    % Update your estimate of (x,y,theta)
    obj.state_estimate.set_pose([x_new, y_new, theta_new]);
end

QuickBot.m

function ir_distances = get_ir_distances(obj)
    % FIX conversion between IR raw and distances, SEE WEEK 2
    ir_array_values = obj.ir_array.get_range();

    %% START CODE BLOCK %%

    ir_voltages = ir_array_values / 500;

    % Manually enter these commands in the console to compute coeff.
    % distances = [0.04,0.05,0.06,0.07,0.08,0.09,0.10,0.12,0.14,0.16,0.18,0.20,0.25,0.30];
    % voltages = [2.750,2.350,2.050,1.750,1.550,1.400,1.275,1.075,0.925,0.805,0.725,0.650,0.500,0.400];
    % coeff = polyfit(voltages, distances, 5);

    % The resulting calculation in the console window gives us the following coeff values.
    coeff = [-0.0182, 0.1690, -0.6264, 1.1853, -1.2104, 0.6293];

    %% END CODE BLOCK %%

    ir_distances = polyval(coeff, ir_voltages);
end