# Robot Fuzzy Logic

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

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;

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;

%--------------------------------------------------------------

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;

disp(['Collision detected in (' num2str(posn(1)) ',' num2str(posn(2)) ') imposible movement.']);

else

posn = posnn;

end

pause(.01);

end

%measures_lrf.m - Obtains the LRF sensors values.

function [rangeL, rangeR, rangeC, rangeLC, rangeRC] = measures_lrf(posn,rad, scenario)

rangeL = rangefinder([posn(1), posn(2), posn(3)+pi/6], rad, scenario); %left

rangeR = rangefinder([posn(1), posn(2), posn(3)-pi/6], rad, scenario); %right

% Sensor in line with the head

rangeLC = rangefinder([posn(1), posn(2), posn(3)+pi/12], rad, scenario); %left

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.

%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

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;

%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)

%

hold off;

imagesc(scenario), axis image off; colormap gray;

hold on;

%draw a little circle for the robot

angs = 0:pi/10:2*pi;

plot(y,x,'k-','LineWidth',3);

%sens = [rangeL, rangeR, rangeC, rangeLC, rangeRC];

tColor = color_from_sensor(sens(1));

drawang([posn(1), posn(2), posn(3) + pi/6],magnitude,tColor);

tColor = color_from_sensor(sens(4));

drawang([posn(1), posn(2), posn(3) + pi/12],magnitude,tColor);

tColor = color_from_sensor(sens(2));

drawang([posn(1), posn(2), posn(3) - pi/6],magnitude,tColor);

tColor = color_from_sensor(sens(5));