---
slideOptions:
transition: slide
---
<style>
.reveal {
font-size: 24px;
}
</style>

# Fuzzy Logic Navigation and Obstacle Avoidance

"Implement Fuzzy Logic Controller in MATLAB and Simulate in V-REP"

For better look check this note, please visit [HackMD.io](https://hackmd.io/@libernormous/flc_nav_obs)

by:
Zaman : [e-mail]

---

# Flow chart of the program


---

# Starting simulation

Detailed explanation of Integrating V-REP with MATLAB can be checked here: [V-Rep tutorial](https://hackmd.io/@libernormous/v-rep)

At the beginning of the program there is always:

vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
vrep.simxFinish(-1); % just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);
if (clientID>-1)
    %//RUN EVERYTHING INSIDE
end

And the program should only run when when MATLAB and V-REP are connected (`if (clientID>-1)`).

---

# Getting object handlers

We have 7 objects that we use in the program.
1. Pioneer-p3dx robot. string ID: `Pioneer_p3dx`
We use this object to find the orientation and position of ROBOT body.
3. Goal object. Flat blue circular object. string ID: `GOAL`
We use this object to find the orientation and position of GOAL body.
5. Right wheel of Pioneer-p3dx. string ID: `Pioneer_p3dx_rightMotor`
We use this object to actuate the right wheel.
7. Left wheel of Pioneer-p3dx. string ID: `Pioneer_p3dx_leftMotor`
We use this object to actuate the left wheel.
9. 3 ultrasound sensors of Pioneer-p3dx. string IDs: `Pioneer_p3dx_ultrasonicSensor2, Pioneer_p3dx_ultrasonicSensor5, Pioneer_p3dx_ultrasonicSensor7`

----

CODE:

%get motor handler
[returnCode,w_hdl(1)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_blocking);
[returnCode,w_hdl(2)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_blocking);
%get ultrasound handler
[returnCode,ultsnd_h(1)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor2',vrep.simx_opmode_blocking);
[returnCode,ultsnd_h(2)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor5',vrep.simx_opmode_blocking); %the angle of this sensor is little bit off
[returnCode,ultsnd_h(3)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor7',vrep.simx_opmode_blocking);
%get robot and GOAL handler
[returnCode,rob_h]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx',vrep.simx_opmode_blocking);
[returnCode,gol_h]=vrep.simxGetObjectHandle(clientID,'GOAL',vrep.simx_opmode_blocking);

---

# Calculating Position Error

1. First we need to get the position of the pioneer robot and the goal object

%%
%get robot pos and orientation
[returnCode,rob_poss]=vrep.simxGetObjectPosition(clientID,rob_h,-1,vrep.simx_opmode_blocking); %ret in meter, vecor
[returnCode,rob_ortt]=vrep.simxGetObjectOrientation(clientID,rob_h,-1,vrep.simx_opmode_blocking); %ret in radian, vector
%get goal pos and orientation
[returnCode,gol_poss]=vrep.simxGetObjectPosition(clientID,gol_h,-1,vrep.simx_opmode_blocking); %ret in meter, vecor
[returnCode,gol_ortt]=vrep.simxGetObjectOrientation(clientID,gol_h,-1,vrep.simx_opmode_blocking); %ret in radian, vecor

2. To get position use function `vrep.simxGetObjectPosition` and `vrep.simxGetObjectOrientation` to get orientation.
3. `rob_poss` return position of the robot in vector $^W\mathbf{p}_R = (R_x, {R_y}, {R_z})$ relative to world frame $\{W\}$. The unit is meter.
4. `rob_ortt` return orientation vector $^W\xi_R = (R_\alpha, R_\beta, R_\gamma)$ relative to world frame $\{W\}$. Where $(\alpha, \beta, \gamma)$ are Euler rotation angles in X, Y, and Z axes respectively. The value ranging from $-\pi$ to $+\pi$. The unit is radian.
5. Same goes with `gol_poss` and `gol_ortt`. It return $^W\mathbf{p}_G = (G_x, {G_y}, {G_z})$ and $^W\xi_G = (G_\alpha, G_\beta, G_\gamma)$ respectively.

----

6. To calculate position error or distance error we can use Euclidean distance between two points.
$$
d = \sqrt{(R_x-G_x)^2+(R_y-G_y)^2+(R_z-G_z)^2}
$$

%position error
vec_pos = gol_poss-rob_poss;
pos_err = sqrt(sum((vec_pos).^2)); %in meter

---

# Calculating Heading Error


1. To caculate Heading error we need to represent the position of coordinate frame of the goal $\{G\}$ relative to robot frame coordinate $\{R\}$.

----

2. We can do this by using transformation matrix in 2D
$$
\begin{align}
^R\tilde{P}_G &= {^RT_W}\ ^W\tilde{P}_G\\
&= {^WT_R}^{-1} \ ^W\tilde{P}_G\\
&=
\left(
\begin{matrix}
\cos(R_\gamma) & -\sin(R_\gamma) & R_x\\
\sin(R_\gamma) & \cos(R_\gamma) & R_y\\
0 &0 &1
\end{matrix}
\right)^{-1}
\ ^W\tilde{P}_G
\end{align}
$$

4. Here we introduce $^W\tilde{P}_G$ and $^R\tilde{P}_G$ as homogenous coordinate notation of $^W{P}_G$ and $^R{P}_G$ respectively, where.
$$
^W\tilde{P}_G =
\left(
\begin{matrix}
G_x\\
G_y\\
1
\end{matrix}
\right)
\ \ \text{and} \ \
^R\tilde{P}_G =
\left(
\begin{matrix}
^RG_x\\
^RG_y\\
1
\end{matrix}
\right)
$$
5. And we find $\gamma_e$ as
$$
\gamma_e = \text{atan2}(^RG_y\ ,\ ^RG_x)
$$
6. The result is in radian angle

----

CODE:

%%
%Goal position relative to robot position to find heading error
fTr = [cos(gamma_r) -sin(gamma_r) rob_poss(1); sin(gamma_r) cos(gamma_r) rob_poss(2); 0 0 1];
gol_2_r_pos = (fTr)\[gol_poss(1); gol_poss(2); 1];
%heading error
hed_err = atan2(gol_2_r_pos(2), gol_2_r_pos(1));
hed_err = rad2deg(hed_err);

---

# Reading sensor state (detecting or not detecting obstacle)

1. The sensor we are using is ultrasound sensor. There are many ultrasound sensors in Pioneer robot. But we use only 3 sensors as stated in Ref. [1].
2. We read the sensor state using `vrep.simxReadProximitySensor()` function.
CODE:

%%
%get sensor data
detectedPoint={};
detectionState=zeros(3,1);
for i=1:3
[returnCode,detectionState(i),detectedPoint{i},detectedObjectHandle,detectedSurfaceNormalVector]=vrep.simxReadProximitySensor(clientID,ultsnd_h(i),vrep.simx_opmode_blocking);
end

3. It returns 2 important values. `detectionState` contain `true` for detecting object and `false` for not detecting object.
4. `detectedPoint` return $3\times1$ vector containing the distance of detected object in X, Y, and Z direction respectively. Here we only use distance in Z direction.

----

6. If the sensor detects and object, we keep the measured value; If not we assign $400$ centimeter as the default value.

    % sensor value constrain from 0 cm to 400 cm
    for i=1:3
        if detectionState(i)
            ultsnd_masure_Zaxs(i) = detectedPoint{i}(3) *100; % ret in centimeter, vector
        else
            ultsnd_masure_Zaxs(i) = 400;
        end
    end

7. To simulate the real ultrasound sensor, we define "object detected" as if one of the sensor measures a value below $80cm$.

    if min(ultsnd_masure_Zaxs)<80 %//OBSTACLE DETECTED, OAFLC control
    
        %% //OAFLC control routine
        %% //Then actuate the wheel
    
    else %//obstacle NOT detected, TFLC control
    
        %% //TFLC control routine
        %% //Then actuate the wheel
        
    end

---

# Fuzzy Control
Detailed explanation of how I create fuzzy logic control can be checked here : [Fuzzy Notes](https://hackmd.io/@libernormous/fuzzy)

----

## Robot to goal distance membership functions

Input value is in meter.

----

## Robot to goal heading angle membership functions

I use the same as in Ref. [1]. The input is in degree angle.

----

## Output of TFLC (Tracking Fuzzy logic control)

I use the same as in Ref. [1]. I assume the output is in $deg/s$.

----

## TFLC Rules

I use the same as in Ref. [1].

----

## Sensor measured distance membership function

I modify the input because it $70cm$ seems to be to late for the robot to respond. Input is in centimeter.

----

## Output of OAFLC (Obstacle Avoidance Fuzzy logic control)

I modify the crisp output since $15 deg/s$ seems to be to fast for the robot to turn.

----

## OAFLC Rules

I use the same as in Ref. [1].

---

# Actuating the wheels

1. To actuate the wheel we use VREP function `vrep.simxSetJointTargetVelocity()`. The function expects a speed input in $deg/s$ unit.
2. NOTE for TFLC control. Because the output of the control seems to be too fast, the robot is slithering like a snake to reach the goal and the robot will not stop at the goal. So, I divide the crisp output by $2$ to slowdown the robot.
Also, I switch the output of left and right output of TFLC control. because, if not, somehow the output is a positive feedback.
CODE:

    %//actuate the wheel
    centeroid_TFLC_L=centeroid_TFLC_L/2; %divide by 2 to slowdown
    centeroid_TFLC_R=centeroid_TFLC_R/2; %divide by 2 to slowdown
    %//somehow I need to flip R and L to make it work, if not it'd be
    %//possitive feedback
    [returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(1),centeroid_TFLC_R,vrep.simx_opmode_blocking);%left
    [returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(2),centeroid_TFLC_L,vrep.simx_opmode_blocking);%right

---

# Stopping simulation

At the end of the simulation there is always an object destructor routine.
CODE:

[returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(1),0,vrep.simx_opmode_blocking);
[returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(2),0,vrep.simx_opmode_blocking);

vrep.delete(); % call the destructor!

---

# Video Results

## TFLC Simulation using V-REP

{%youtube tAe7P6DFXdQ %}

----

## OAFLC Simulation using V-REP

{%youtube qADtNtXeQIE %}

----

## TFLC and OAFLC combined

{%youtube Q6hWn8U3ps0 %}

---

# Graph Results of (TFLC only)

----

## Robot position


----

## Angular velocity of both wheels

Blue line is angular velocity of right wheel. Red line is angular velocity of left wheel. Both values are in $deg/s$. Cyan dotted lines is the state of detection.

----

## Robot velocity

Black line is velocity magnitude of the robot in $m/s$. Cyan dotted lines is the state of detection.

----

## Robot heading

Black line is heading of the robot in $deg$. Cyan dotted lines is the state of detection.

----

## Robot angular velocity

Black line is angular velocity magnitude of the robot in $deg/s$. Cyan dotted lines is the state of detection.

---

# Graph Results of (TFLC and OAFLC combined)

----

## Robot position


----

## Angular velocity of both wheels

Blue line is angular velocity of right wheel. Red line is angular velocity of left wheel. Both values are in $deg/s$. Cyan dotted lines is the state of detection.

----

## Robot velocity

Black line is velocity magnitude of the robot in $m/s$. Cyan dotted lines is the state of detection.

----

## Robot heading

Black line is heading of the robot in $deg$. Cyan dotted lines is the state of detection.

----

## Robot angular velocity

Black line is angular velocity magnitude of the robot in $deg/s$. Cyan dotted lines is the state of detection.

---

# MATLAB CODE

disp('Program started');

%TFLC RULES
rules_TFLC_L = [
    1 1 1 1 3 3 4;
    2 2 1 2 3 5 6;
    2 2 2 3 5 6 7;
    1 1 1 4 6 6 7;
    2 2 3 5 5 6 7;
    1 1 4 6 5 6 7;
    2 2 3 7 5 6 7
    ];
rules_TFLC_L_var = {'z'; 's'; 'nm'; 'm'; 'nh'; 'h'; 'vh'};
rules_TFLC_R = flip(rules_TFLC_L,2);

%OAFLC RULES
rules_OAFLC_R_L = [
    1 1;
    2 1;
    2 1;
    1 1;
    2 1;
    2 1;
    1 1;
    2 1;
    2 1;
    1 2;
    1 1;
    5 3;
    3 5;
    5 3;
    5 3;
    1 2;
    5 3;
    5 3;
    1 2;
    3 5;
    1 1;
    1 2;
    3 5;
    5 3;
    1 2;
    3 5;
    4 4;
    ];
var_OAFLC_output={'nh', 'n', 'p', 'hp', 'vhp'};

vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
vrep.simxFinish(-1); % just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);

Ts = 0.05; %0.01 s = 10 ms %default simulation dt on vrep is 50 ms
w_hdl = zeros(2,1);
v_w = ones(2,1).*60*Ts; %degree/s convert to nominal/Ts
ultsnd_h = zeros(3,1);
ultsnd_masure_Zaxs = zeros(3,1);

if (clientID>-1)
    disp('Connected to remote API server')
    
    %get motor handler
    [returnCode,w_hdl(1)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_blocking);
    [returnCode,w_hdl(2)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_blocking);
    %get ultrasound handler
    [returnCode,ultsnd_h(1)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor2',vrep.simx_opmode_blocking);
    [returnCode,ultsnd_h(2)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor5',vrep.simx_opmode_blocking); %the angle of this sensor is little bit off
    [returnCode,ultsnd_h(3)]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor7',vrep.simx_opmode_blocking);
    %get robot and GOAL handler
    [returnCode,rob_h]=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx',vrep.simx_opmode_blocking);
    [returnCode,gol_h]=vrep.simxGetObjectHandle(clientID,'GOAL',vrep.simx_opmode_blocking);
    
    n=0;
    while n<200
        %-----getting data-----
        %get robot pos and orientation
        [returnCode,rob_poss]=vrep.simxGetObjectPosition(clientID,rob_h,-1,vrep.simx_opmode_blocking); %ret in meter, vecor
        [returnCode,rob_ortt]=vrep.simxGetObjectOrientation(clientID,rob_h,-1,vrep.simx_opmode_blocking); %ret in radian, vector
        %get goal pos and orientation
        [returnCode,gol_poss]=vrep.simxGetObjectPosition(clientID,gol_h,-1,vrep.simx_opmode_blocking); %ret in meter, vecor
        [returnCode,gol_ortt]=vrep.simxGetObjectOrientation(clientID,gol_h,-1,vrep.simx_opmode_blocking); %ret in radian, vecor
        %get sensor data
        detectedPoint={};
        detectionState=zeros(3,1);
        for i=1:3
            [returnCode,detectionState(i),detectedPoint{i},detectedObjectHandle,detectedSurfaceNormalVector]=vrep.simxReadProximitySensor(clientID,ultsnd_h(i),vrep.simx_opmode_blocking);
        end
        %-----processing data-----
        %position goal error / vector magnitude, and orientation error
        %position error
        vec_pos = gol_poss-rob_poss;
        pos_err = sqrt(sum((vec_pos).^2)); %in meter
        %orientation error
        gamma_r = rob_ortt(3);
        gamma_g = gol_ortt(3);
        rob_x_2_g_or = [cos(gamma_r) sin(gamma_r); -sin(gamma_r) cos(gamma_r)]*[cos(gamma_g) -sin(gamma_g); sin(gamma_g) cos(gamma_g)]*[1;0];
        orr_err = atan2(rob_x_2_g_or(2), rob_x_2_g_or(1));
        orr_err = rad2deg(orr_err);
        %Goal position relative to robot position to find heading error
        fTr = [cos(gamma_r) -sin(gamma_r) rob_poss(1); sin(gamma_r) cos(gamma_r) rob_poss(2); 0 0 1];
        gol_2_r_pos = (fTr)\[gol_poss(1); gol_poss(2); 1];
        %heading error
        hed_err = atan2(gol_2_r_pos(2), gol_2_r_pos(1));
        hed_err = rad2deg(hed_err);
        % sensor value constrain from 0 cm to 400 cm
        for i=1:3
            if detectionState(i)
                ultsnd_masure_Zaxs(i) = detectedPoint{i}(3) *100; % ret in centimeter, vector
            else
                ultsnd_masure_Zaxs(i) = 400;
            end
        end
        %-----Controller START here-----
        if min(ultsnd_masure_Zaxs)<60 %OBSTACLE DETECTED, OAFLC control
            %----------OAFLC control
            %fuzzyfication
            fuz_s1 = check_obstacle_membership(ultsnd_masure_Zaxs(1)); %left sensor
            fuz_s2 = check_obstacle_membership(ultsnd_masure_Zaxs(2)); %front sensor
            fuz_s3 = check_obstacle_membership(ultsnd_masure_Zaxs(3)); %right sensor
            %inference
            [mat_rules_val,v_OAFLC_L_th, v_OAFLC_R_th] = relate_OAFLC (fuz_s1, fuz_s2, fuz_s3, rules_OAFLC_R_L);
            %calculating centroid OAFLC R
            dt = 0.01;
            t = -10:dt:15; %output data range
            out = [];
            for i=1:length(t)
                ret=check_vl_vr_oaflc_membership(t(i)); %output fuzzification
                ret = min(v_OAFLC_R_th, ret);
                %Max value / real value of the inference
                out = [out max(ret)]; %MAX
            end
            centeroid_OAFLC_R = sum(out.*t)/sum(out)
            %calculating centroid OAFLC L
            out = [];
            for i=1:length(t)
                ret=check_vl_vr_oaflc_membership(t(i)); %output fuzzification
                ret = min(v_OAFLC_L_th, ret);
                %Max value / real value of the inference
                out = [out max(ret)]; %MAX
            end
            centeroid_OAFLC_L = sum(out.*t)/sum(out)
            %actuate the wheel
            [returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(1),centeroid_OAFLC_L,vrep.simx_opmode_blocking);%left
            [returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(2),centeroid_OAFLC_R,vrep.simx_opmode_blocking);%right
        else %obstacle NOT detected, TFLC control
            %----------TFLC control
            %fuzzyfication
            fuz_dist = check_distance_membership(pos_err);
            fuz_orie = check_err_angle_membership(hed_err);
            %inference
            [mat_TFLC_L_rel,v_TFLC_L_th] = relate_TFLC (fuz_dist, fuz_orie, rules_TFLC_L);
            [mat_TFLC_R_rel,v_TFLC_R_th] = relate_TFLC (fuz_dist, fuz_orie, rules_TFLC_R);
            %calculating centroid TFLC L
            dt = 0.01;
            t = -1:dt:13; %output data range
            out = [];
            for i=1:length(t)
                ret=check_velocity_membership(t(i)); %output fuzzification
                ret = min(v_TFLC_L_th, ret);
                %Max value / real value of the inference
                out = [out max(ret)]; %MAX
            end
            centeroid_TFLC_L = sum(out.*t)/sum(out);
            %calculating centroid TFLC R
            out = [];
            for i=1:length(t)
                ret=check_velocity_membership(t(i)); %output fuzzification
                ret = min(v_TFLC_R_th, ret);
                %Max value / real value of the inference
                out = [out max(ret)]; %MAX
            end
            centeroid_TFLC_R = sum(out.*t)/sum(out);
            %actuate the wheel
            centeroid_TFLC_L=centeroid_TFLC_L/2;
            centeroid_TFLC_R=centeroid_TFLC_R/2;
            %somehow I need to flip R and L to make it work, if not it'd be
            %possitive feedback
            [returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(1),centeroid_TFLC_R,vrep.simx_opmode_blocking);%left
            [returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(2),centeroid_TFLC_L,vrep.simx_opmode_blocking);%right
        end
        %-----Controller END here-----
        %increamnet counter
        n=n+1;
        %BREAK LOOP BY BUTTON
        ButtonHandle = uicontrol('Style', 'PushButton', ...
                                 'String', 'Stop loop', ...
                                 'Callback', 'delete(gcbf)');
        drawnow;
        if ~ishandle(ButtonHandle)
            disp('Loop stopped by user');
            break;
        end
    end
    
    vrep.simxFinish(clientID);
else
    disp('Failed connecting to remote API server');
end

[returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(1),0,vrep.simx_opmode_blocking);
[returnCode]=vrep.simxSetJointTargetVelocity(clientID,w_hdl(2),0,vrep.simx_opmode_blocking);

vrep.delete(); % call the destructor!

disp('Program ended');


%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                      FUNCTIONS
%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% =====================to check all value in distance fuzzification
function out = check_distance_membership(input)
    z = triangle (input, 0, 0, 0.50, 1, 1, 0); %zero
    nz = triangle (input, 0, 0.50, 1.00, 0, 1, 0); %near zero
    n = triangle (input, 0.50, 1.00, 1.50, 0, 1, 0); %near
    m = triangle (input, 1.00, 1.50, 2.00, 0, 1, 0); %medium
    nf = triangle (input, 1.50, 2.00, 2.50, 0, 1, 0); %near far
    f = triangle (input, 2.00, 2.50, 3.00, 0, 1, 0); %far
    vf = triangle (input, 2.50, 3.00, 3.00, 0, 1, 1); %very far
    out = [z;nz;n;m;nf;f;vf];
end
%% =====================to check all value in angle error fuzzification
function out = check_err_angle_membership(input)
    n = triangle (input, -175, -175, -88, 1, 1, 0); %negative
    sn = triangle (input, -175, -88, -43, 0, 1, 0); %small neg
    nnz = triangle (input, -88, -43, 0, 0, 1, 0); % near neg zero
    z = triangle (input, -43, 0, 43, 0, 1, 0); %zero
    npz = triangle (input, 0, 43, 88, 0, 1, 0); %near pos zero
    sp = triangle (input, 43, 88, 175, 0, 1, 0); %small pos
    p = triangle (input, 88, 175, 175, 0, 1, 1); %positive
    out = [n; sn; nnz; z; npz; sp; p];
end
%% =====================to check all value in velocity of motors in TFLC fuzzification
function out = check_velocity_membership(input)
    z = triangle (input, 0, 0, 2, 1, 1, 0);%zero
    s = triangle (input, 0, 2, 4, 0, 1, 0);%slow
    nm = triangle (input, 2, 4, 6, 0, 1, 0);%near medium
    m = triangle (input, 4, 6, 8, 0, 1, 0);%medium
    nh = triangle (input, 6, 8, 10, 0, 1, 0);%near high
    h = triangle (input, 8, 10, 12, 0, 1, 0);%high
    vh = triangle (input, 10, 12, 12, 0, 1, 1);%very high
    out = [z;s;nm;m;nh;h;vh];
end
%% =====================to check all value from sensor
function out = check_obstacle_membership(input)
    n = trapezoid (input, 0, 10, 30, 40, 0, 1, 1, 0);%near
    m = trapezoid (input, 30, 40, 60, 70, 0, 1, 1, 0);%medium
    f = trapezoid (input, 60, 70, 90, 100, 0, 1, 1, 1);%far
    out = [n;m;f];
end
%% =====================to check all value in LV and RV of OAFLC
function out = check_vl_vr_oaflc_membership(input)
    nh = triangle (input, -10, -5, 0, 0, 1, 0);%negative high
    n = triangle (input, -5, -2.5, 0, 0, 1, 0);%negative
    p = triangle (input, 0, 2.5, 5, 0, 1, 0);%positive
    hp = triangle (input, 0, 5, 10, 0, 1, 0);%high positive
    vhp = triangle (input, 5, 7.5, 10, 0, 1, 0);%very high positive
    out = [nh;n;p;hp;vhp];
end
%% Relate a fuzzification to another with the result of matrix & vec
% the relation is using minimum, because the relation in fuzzy rules are
% using AND
function [mat_rules_val,v_out_th] = relate_TFLC (set1, set2, rules_index)
    % set1 is rows, and set2 is columns
    % find correspoding values for each rules
    m = zeros(length(set1), length(set2));
    for i=1:length(set1)
        for j=1:length(set2)
            m(i,j)=min(set1(i),set2(j)); %MIN
        end
    end
    
    % find the output index of each rules values
    % then create vector, of possible maximum mambership value from output
    % membership class
    v_out = zeros(max(max(rules_index)),1);
    [h,w]=size(rules_index);
    for i = 1:h
        for j = 1:w
            v_out( rules_index(i,j) ) = max(v_out( rules_index(i,j) ), m (i,j)); %MAX
        end
    end
    mat_rules_val = m;
    v_out_th = v_out;
end
%% Relate a fuzzification to another with the result of rules table & vec
% the relation is using minimum, because the relation in fuzzy rules are
% using AND
function [mat_rules_val,v_out_th1, v_out_th2] = relate_OAFLC (set1, set2, set3, rules_index)
    % find correspoding values for each rules
    m = zeros(size(rules_index));
    c = 0;
    for i=1:length(set1)
        for j=1:length(set2)
            for k=1:length(set3)
                c = c+1;
                MIN = min(min(set1(i),set2(j)),set3(k));
                m(c,1)=MIN; %MIN output1
                m(c,2)=MIN; %MIN output2 
            end
        end
    end
    
    % find the output index of each rules values
    % then create vector, of possible maximum mambership value from output
    % membership class
    v_out1 = zeros(max(rules_index(:,1)),1); % vector output1
    v_out2 = zeros(max(rules_index(:,2)),1); % vector output2
    [h,w]=size(rules_index);
    for i = 1:h
        v_out1( rules_index(i,1) ) = max(v_out1( rules_index(i,1) ), m(i,1)); %MAX
        v_out2( rules_index(i,2) ) = max(v_out2( rules_index(i,2) ), m(i,2)); %MAX
    end
    mat_rules_val = m;
    v_out_th1 = v_out1;
    v_out_th2 = v_out2;
end
%% ===============triangle shaped fuzzy membership
function out = triangle (input, start, middle, stop, str_val, mid_val, sto_val)
    if input<start
        out = str_val;
        
    elseif (input>=start) && (input<middle)
        v = (mid_val-str_val)/(middle-start);
        out = str_val + v*(input-start);
    
    elseif (input==middle)
        out = mid_val;
        
    elseif (input>middle) && (input<=stop)
        v = (sto_val-mid_val)/(stop-middle);
        out = mid_val + v*(input-middle);
        
    elseif input>stop
        out = sto_val;
    end
end
%% ===============trapezoid shaped fuzzy membership
function out = trapezoid (input, start, middle1, middle2, stop, str_val, mid1_val, mid2_val, sto_val)
    if input<start
        out = str_val;
        
    elseif (input>=start) && (input<middle1)
        v = (mid1_val-str_val)/(middle1-start);
        out = str_val + v*(input-start);
    
    elseif (input==middle1)
        out = mid1_val;
        
    elseif (input>middle1) && (input<middle2)
        v = (mid2_val-mid1_val)/(middle2-middle1);
        out = mid1_val + v*(input-middle1);
        
    elseif (input==middle2)
        out = mid2_val;
        
    elseif (input>middle2) && (input<=stop)
        v = (sto_val-mid2_val)/(stop-middle2);
        out = mid2_val + v*(input-middle2);
        
    elseif input>stop
        out = sto_val;
    end
end

---

# References

[1] Faisal, Mohammed, et al. "Fuzzy logic navigation and obstacle avoidance by a mobile robot in an unknown dynamic environment." International Journal of Advanced Robotic Systems 10.1 (2013): 37.

###### tags: `fuzzy logic` `matlab` `vrep` `robot`

---

# Special thanks to
1. Professor Hsiu-Ming Wu of Taipei Tech