Roam function
function [vL, vR] = roam(rangeL, rangeR, rangeC, rangeLC, rangeRC, varargin)
vL_target = 5;
vR_target = 5;
range = min([rangeL rangeR rangeC rangeLC rangeRC]);
if(range <= 10)
vL = vL_target;
vR = vR_target-10+range;
else
vL = vL_target;
vR = vR_target+2*(.5-rand);
end
Evalfis function detailed description
Fuzzy Control function
function [vL, vR] = ctr_fuzzy_1(rangeL, rangeR, rangeC, rangeLC, rangeRC, varargin)
if length(varargin) < 1
error('Error, a parameter must be specified together with the fuzzy controller');
end;
fis = varargin{1};
[outs] = evalfis([rangeL, rangeC, rangeR],fis);
vL = outs(1,1);
vR = outs(1,2);
Robot Starting function
function [pos, sensors] = start_robot(imagen_png, funcion_control, dt, t_end)
if nargin < 1
imagen_png = 'salon.png';
end;
posn = [0,0,0]; % ydim, xdim, angle
rad = 4; % Radius of the robot (pixels)
wdia = 7; % Distance between robot's wheels (pixels)
if nargin < 2
funcion_control = 'roam'; % Robot's control function
end;
if nargin < 3
dt = .5; % Simulation time increment
end;
if nargin < 4
t_end = 100; % Simulation final time
end;
scenario = rgb2gray(imread(imagen_png));
imagesc(scenario), axis image off; colormap gray;
title('Click to specify the robot's initial position');
disp('Click in the scenario to specify the robot's initial position ... ');
[posn(1) posn(2)] = ginput(1);
title('Click to specify the robot's initial orientation ...');
disp('Click on the scenario to specify the robot's initial orientation ...' ... ');
[y x] = ginput(1);
y = y - posn(1);
x = x - posn(2);
[posn(3), r] = cart2pol(x,y);
[pos, sensors] = simula_robot(posn, rad, wdia, scenario,funcion_control, dt, t_end);
% Robot's trayectory trace plot
hold on
plot(pos(:,1),pos(:,2),'y-');
% pos
% sensors
clear fis
Main function
if nargin < 5
error('A parameter is missing');
end;
if nargin < 5, dt = 0.1; end;
if nargin < 6, t_end = 1000; end;
simulation_times = 1:dt:t_end;
if nargout >= 1
num_filas = length(simulation_times) + 1;
pos = zeros(num_filas, size(posn,2));
pos(1,:) = posn;
end;
if nargout >= 2
sensores = zeros(num_filas, 5);
end;
fila = 2;
%--------------------------------------------------------------
controller_fis = readfis('controla_robot_1.fis');
for i = tiempos_simulacion
[rangeL, rangeR, rangeC, rangeLC, rangeRC] = medidas_lrf(posn, rad, scenario);
sen = [rangeL, rangeR, rangeC, rangeLC, rangeRC];
if nargout >= 2
if i == 1
sensores(1,:) = [ rangeL, rangeR, rangeC, rangeLC, rangeRC];
end;
sensores(fila,:) = [ rangeL, rangeR, rangeC, rangeLC, rangeRC];
end;
[vL, vR] = feval(control_function, rangeL, rangeR, rangeC, rangeLC, rangeRC, controller_fis);
posnn = drive(posn, wdia, vL, vR, dt);
if nargout >= 1
pos(fila,:) = posn;
end;
fila = fila + 1;
if(detectcollision(posnn,rad,scenario))
disp(['Collision detected in (' num2str(posn(1)) ',' num2str(posn(2)) ') imposible movement.']);
else
posn = posnn;
end
drawbot(posn,rad,scenario, sen);
pause(.01);
end
%measures_lrf.m - Obtains the LRF sensors values.
function [rangeL, rangeR, rangeC, rangeLC, rangeRC] = measures_lrf(posn,rad, scenario)
% Sensor at pi/6 radians of the head
rangeL = rangefinder([posn(1), posn(2), posn(3)+pi/6], rad, scenario); %left
% Sensor at -pi/6 radians of the head
rangeR = rangefinder([posn(1), posn(2), posn(3)-pi/6], rad, scenario); %right
% Sensor in line with the head
rangeC = rangefinder(posn, rad,scenario); %center
% Sensor at pi/12 radians of the head (between rangeL and head)
rangeLC = rangefinder([posn(1), posn(2), posn(3)+pi/12], rad, scenario); %left
% Sensor at -pi/12 radians of the head (between rangeR and head)
rangeRC = rangefinder([posn(1), posn(2), posn(3)-pi/12], rad, scenario); %right
%rangefinder.m - Obtains the distance to the nearest object.
% written by: Shawn Lankton
% for: ECE8843 (sort-of) fun (mostly)
function dist = rangefinder(posn, rad, scenario)
% get size of scenario
[dimx, dimy] = size(scenario);
%for 100 steps.
for i = rad:(100+rad)
%get x&y coords of point to test
y = posn(1)+ i*sin(posn(3));
x = posn(2)+ i*cos(posn(3));
%make sure x & y are inside the scenario
if x < 1, x = 1; end;
%x(x < 1) = 1;
if x > dimx, x = dimx; end;
if y < 1, y = 1; end;
if y > dimy, y = dimy; end;
%see if the scenario at x & y is an obstacle or not
if(scenario(round(x),round(y)) == 0)
%if yes, return the distance to the obstacle
dist = i-rad;
return;
end
end
%if no obstacles found within 100 pixels, return 100
dist = 100;
return;
%drive.m - does the dead rekoning kinematics based on wheel speeds
% written by: Shawn Lankton
% for: ECE8843 (sort-of) fun (mostly)
function [newposn] = drive(posn, wdia, vL, vR, t)
%get speed differences & sums
vdiff = vR-vL;
vsum = vR+vL;
%calculate new angle (pretty simple)
newposn(3) = posn(3) + vdiff*t/wdia;
if(vdiff == 0)
%calculate new [y x] if wheels moving together.
newposn(1) = vL*t*sin(posn(3))+posn(1);
newposn(2) = vR*t*cos(posn(3))+posn(2);
else
%calculate new [y x] if wheels moving at unequal speeds.
newposn(1) = posn(1) - wdia*vsum/(2*vdiff)*(cos(vdiff*t/wdia+posn(3))-cos(posn(3)));
newposn(2) = posn(2) + wdia*vsum/(2*vdiff)*(sin(vdiff*t/wdia+posn(3))-sin(posn(3)));
end
%detectcollisions.m - sees if the robot hit something
% written by: Shawn Lankton
% for: ECE8843 (sort-of) fun (mostly)
%
function bool = detectcollision(posn, rad, scenario)
%get a circle representing the robot's body
angs = 0:pi/10:2*pi;
y = posn(1) + rad*sin(angs);
x = posn(2) + rad*cos(angs);
%for each of these 20 pixels, check if one is on black
for i = 1:length(y)
if(scenario(round(x(i)),round(y(i))) == 0)
%if so, return 1
bool = 1;
return;
end
end
%if not, return 0
bool = 0;
%drawang.m - Draws a straight line using as parameters, the initial position, %an angle (obtained both from posn vector) and a magnitude (mag)
function drawang(posn, mag, tColor)
% do some polar to cartesian math
y = [posn(1), posn(1)+ mag*sin(posn(3))];
x = [posn(2), posn(2)+ mag*cos(posn(3))];
plot(y,x,tColor);
function tColor = color_from_sensor(range)
if range > 70
tColor = 'g--';
elseif range > 50
tColor = 'm--';
elseif range > 30
tColor = 'b--';
elseif range > 15
tColor = 'k-';
else
tColor = 'r-';
end;
%drawbot.m - plots the robot & its wiskers on the scenario
% written by: Shawn Lankton
% for: ECE8843 (sort-of) fun (mostly)
%
function drawbot(posn, rad, scenario,sens)
hold off;
imagesc(scenario), axis image off; colormap gray;
hold on;
%draw a little circle for the robot
angs = 0:pi/10:2*pi;
y = posn(1) + rad*sin(angs);
x = posn(2) + rad*cos(angs);
plot(y,x,'k-','LineWidth',3);
%sens = [rangeL, rangeR, rangeC, rangeLC, rangeRC];
tColor = color_from_sensor(sens(1));
magnitude = rad + min(sens(1),100);
%magnitude = rad + 10;
drawang([posn(1), posn(2), posn(3) + pi/6],magnitude,tColor);
tColor = color_from_sensor(sens(4));
magnitude = rad + min(sens(4),100);
drawang([posn(1), posn(2), posn(3) + pi/12],magnitude,tColor);
tColor = color_from_sensor(sens(2));
magnitude = rad + min(sens(2),100);
drawang([posn(1), posn(2), posn(3) - pi/6],magnitude,tColor);
tColor = color_from_sensor(sens(5));
magnitude = rad + min(sens(5),100);
drawang([posn(1), posn(2), posn(3) - pi/12],magnitude,tColor);
tColor = color_from_sensor(sens(3));
magnitude = rad + min(sens(3),100);
drawang([posn(1), posn(2), posn(3) ],magnitude,tColor);