Faculty of Power and Aeronautical Engineering

Department of Automatic control & Robotics

Mobile Robot Lab Report

Prepared by – Yigezu, Eyob Azene

14 April 2021
Task 1.b: Moving along a circle trajectory with constant orientation.
This task implements the trajectory of the youBot robot which has to follow a circle.
The robot should keep the constant orientation while moving along a circle. The radius
of the circle is passed to the control program through the variable-length argument
list in the run_simulation function (in this code radius = 1.234m) and the running
argument used is run_simulation(@solution1b, false, 1.234).

The control program is implemented as a two-state finite state machine (initial and
move) the latter is responsible for computation of the required linear and angular
velocities. At the initial state, the coordinate of the circle center is stored. And the
robot moves infinitely (no stopping condition)

CODE
function [forwBackVel, leftRightVel, rotVel, finish] = solution1b(pts,
contacts, position, orientation, varargin)
% The control loop callback function - the solution for task 1B

% get the destination point

if length(varargin) ~= 1
error('Wrong number of additional arguments: %d\n', length(varargin));
end

% desired circle radius

radius = abs(varargin{1});

% current position of the robot x, y

position = position(1:2);

% max velocity limit

vel_limit = 5;

% declare the persistent variable that keeps the state of the Finite
% State Machine (FSM)
persistent state;
if isempty(state)
% the initial state of the FSM is 'init'
state = 'init';
end

% initialize the robot control variables (returned by this function)

finish = false;
forwBackVel = 0;
leftRightVel = 0;
rotVel = 0;

% manage the states of FSM

persistent circleCenter;
 if strcmp(state, 'init')
state = 'move';
fprintf('changing FSM state to %s\n', state);
circleCenter = [position(1) + radius, position(2)];
elseif strcmp(state, 'move')
% Calculate the distance from the circle
distance = calculate_norm(position);
radius_err = ( radius - distance);
vel_radial = [(position(1)-circleCenter(1)) (position(2)-
circleCenter(2))];
vel_tang = vel_radial* [0 -1; 1 0];

theta = orientation(3);

vel_G = 2*((vel_radial*radius_err) + vel_tang);

% Convert velocities from the global frame to the local frame

vel_L = transformation(vel_G, theta);

leftRightVel = vel_L(1);
forwBackVel = vel_L(2);
rotVel = 0;

if leftRightVel > vel_limit

leftRightVel = vel_limit;
end
if leftRightVel < -vel_limit
leftRightVel = -vel_limit;
end
if forwBackVel > vel_limit
forwBackVel = vel_limit;
end
if forwBackVel < -vel_limit
forwBackVel = -vel_limit;
end
else
error('Unknown state %s.\n', state);
end
end

% Calculate norm
function norm = calculate_norm(pos)
% calculate the norm of distance
norm = sqrt(pos(1)^2 + pos(2)^2);
end

% transform from global to local

function L_Vel = transformation(G_Vel, theta)

% transform the global frame to the local frame

R = [cos(theta), -sin(theta); sin(theta), cos(theta)];

% Returns local velocity

L_Vel = G_Vel * R;
end
RESULT
The radius of the circle is passed to the control program through the variable-length argument
list in the run_simulation function run_simulation(@solution1b, false, 1.234). .And
the output looks like

As it is seen from the above pictures the robot follows circular trajectory infinitely without
changing the orientation of the robot.