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

# Fuzzy Logic Control Simulation on MATLAB and Simulink

*"We implement Dynamic model on simulink and put FLC only using MATLAB code"*

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

by:
[Liber Normous](https://hackmd.io/@libernormous)

---

# Fuzzy Logic control

1. The Fuzzy Logic Control is based on Ref [1].
2. We have a tutorial for creating Fuzzy Logic Control (FLC) only using MATLAB code [here](https://hackmd.io/@libernormous/fuzzy)
3. This is the second version of FLC implementation. Previously we implemented the FLC on V-REP. The tutorial can be found [here](https://hackmd.io/@libernormous/flc_nav_obs).

---

# Dynamic model of the robot

1. The dynamic model of the robot is based on Ref [2].
2. We have a tutorial to build this dynamic model on Simulink. It can be found in [here](https://hackmd.io/@libernormous/dynamic_ddmr).
3. We will use the previous model and modify it. We add FLC in it.

---

# Overall simulink model

Open the image in new tab if it is too small.

In this model add `pos_orr_err` block. That is a block to measure the distance between the robot and goal. It also calculate the heading error of the robot.

The other block is TFLC block which is Tracking Fuzzy Logic Control from Ref. [1].

---

# Calculating position and orientation error


This is the code inside the block:

function [pos_err,orr_err] = pos_orr_err(R_x, R_y, R_g, G_x, G_y, G_g)
    %values assignment
    rob_poss = [R_x; R_y]; %in meter
    gol_poss = [G_x; G_y]; %in meter
    gamma_r = R_g;         %in radian
%     % convert radian to -pi to pi values
%     if gamma_r>pi
%         gamma_r = gamma_r-2*pi;
%     elseif gamma_r<pi
%         gamma_r = gamma_r+2*pi;
%     end

    %position error
    vec_pos = gol_poss-rob_poss;
    pos_err = sqrt(sum((vec_pos).^2)); %in meter
    
    %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);
    orr_err = hed_err; %in degree angle
end

----

## Position error

The position error is calculated using Euclidean distance:
$$
d = \sqrt{(R_x-G_x)^2+(R_y-G_y)^2}
$$
That is why we need the position of the robot $(R_x, R_y)$ and the position of the goal $(G_x, G_y)$. Here we do the simulation on 2D plane. More explanation [here](https://hackmd.io/@libernormous/flc_nav_obs#Calculating-Position-Error).

----

## Heading error

All the calculation of heading error are in $rad$ degree. To calculate heading error, we need $G_x, G_y, R_x, R_y$ and $R_\gamma$ which is orientation of the robot. We will represent the pose of the goal in the robot frame.
To do this we need a transformation matrix.
$$
\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\\
\left(
\begin{matrix}
^RG_x\\
^RG_y\\
1
\end{matrix}
\right)&=\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} \left(
\begin{matrix}
^WG_x\\
^WG_y\\
1
\end{matrix}
\right)
\end{align}
$$
And we find the orientation as:
$$
\gamma_e = \text{atan2}(^RG_y\ ,\ ^RG_x)
$$
This explanation can be found in [here](https://hackmd.io/@libernormous/flc_nav_obs#Calculating-Heading-Error).

---

# TFLC block

This is what inside the block:

function [v_r,v_l] = tflc(position_error,orientation_error)
    %% global constant
    %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_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;
    ];

    %% run all nested function
    %fuzzyfication
    fuz_dist = check_distance_membership(position_error); % in meter
    fuz_orie = check_err_angle_membership(orientation_error); % in degree angle
    %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
    v_r = centeroid_TFLC_L; %because the rules is inverted
    v_l = centeroid_TFLC_R;
    
    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%                      FUNCTIONS
    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% =====================to check all value in distance fuzzification
    function out = check_distance_membership(input) %meter
        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
    %% ===============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;
        else
            out = 0;
        end
    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
end

Essentially it is the same as our [previous model](https://hackmd.io/@libernormous/flc_nav_obs), but we simplify it to a function of TFLC only.

---

# Running the simulation

To run the simulation, we need another file to initialize all the parameters. We have done this before in [previous note](https://hackmd.io/@libernormous/dynamic_ddmr#Parameters).

clear;
%% Parameters
La=0.94; %H, inductance of the armature winding
Ra=1; %Ohm, resistance of the armature winding
Kb= 1;%0.0301; %V/rad/s the back emf constant 

N=1;  %the gear ratio
Kt=0.0300; %Nm/Amp the torque constant

J = 2; % Moment inertia of robot KG*m^2
M = 5; % Robot mass in KG
d = 0.02; % Location of center of gravity of robot. x=d y=o in robot frame
R = 0.02;%radius of wheel in meter
L = 0.1;%distance between wheel in meter


%% Input
V_r = 1; % right motor voltage
V_l = 0; % right motor voltage

G_x = 1; %Goal x position
G_y = -1; %Goal y position
G_g = pi/2; %Goal z orientation

xa=0;
ya=0;
theta=0;

vol_gain = 1; % to slowdown the robot

TF = 40; %simulation time

%% Run simulation
simout = sim('func1_v4');
figure(3);
subplot(3,3,1),plot(simout.tout, simout.ya), xlabel('t(s)'), ylabel('ya(m)');
subplot(3,3,2),plot(simout.tout, simout.xa), xlabel('t(s)'), ylabel('xa(m)');
subplot(3,3,3),plot(simout.xa, simout.ya, G_x, G_y, '*'), xlabel('xa(m)'), ylabel('ya(m)');
subplot(3,3,4),plot(simout.tout, simout.theta), xlabel('t(s)'), ylabel('theta(rad)');
subplot(3,3,5),plot(simout.tout, simout.poser), xlabel('t(s)'), ylabel('position error(m)');
subplot(3,3,6),plot(simout.tout, simout.orrer), xlabel('t(s)'), ylabel('heading error(deg)');
subplot(3,3,7),plot(simout.tout, simout.vr_flc), xlabel('t(s)'), ylabel('right motor armature (volt)');
subplot(3,3,8),plot(simout.tout, simout.vl_flc), xlabel('t(s)'), ylabel('left motor armature (volt)');
subplot(3,3,9),plot(simout.tout, simout.vl_flc, 'r', simout.tout, simout.vr_flc, 'b'), xlabel('t(s)'), ylabel('armature (volt)');

In above code we use motor model C-23-L33-W20 (winding code 20) from Ref. [[3]](https://www.moog.com/content/dam/moog/literature/MCG/moc23series.pdf). **BUT** we use $K_b=1{v/rad/s}$. This is to make the wheel stop faster.

---

# Result 1

From previous code we can create simulations of many goals references. For example we make 9 simulation with sequence of $(G_x, G_y)$ in this matrix.
$$
\left(\begin{matrix}
G_x\\
G_y
\end{matrix}\right)=\left(\begin{matrix}
-1 & 0 & 1 & -1 & 0 & 1 & -1 & 0 & 1\\
1 & 1 & 1 & 0 & 0 & 0 & -1 & -1 & -1
\end{matrix}\right)
$$

----

The **stars** are the goals. The middle graph is the result of all the graphs combined. In these images we know that the robot reach the goal, but we are not sure for how long the simulation has been run. We will have a closer look of the heading and distance error in the next section.

----

Here is the result of the robot approaching $G_x=-1m, G_y=-1m$. As we see, the position error are decreasing without problem, but the orientation is oscillating. So the robot is slithering like a snake. We can also see that the the armature voltages are dropping near the goal. The simulation time is $40s$.

----

Here is another result when the goal is $G_x=-2.5m, G_y=0m$.

----

We move the goal to $G_x=-15m, G_y=0m$, and $40s$ of simulation is not enough anymore.

---

# Result 2 (Using real motor parameter)

We don't simulate the friction force. We chose $K_b=1{v/rad/s}$ because it is the only thing that will stop the motors. If we use the real motor parameter values from datasheets, the results are quite unsatisfying.
Here we change $K_b=0.0301{v/rad/s}$ to match the model C-23-L33-W20 (winding code 20) from Ref. [[3]](https://www.moog.com/content/dam/moog/literature/MCG/moc23series.pdf).

----

This is when $G_x=-25m, G_y=0m$

----

And this is when $G_x=30m, G_y=3m$.

----

And to show the dullness of the control system (or maybe the simulation) we set $G_x=100m, G_y=0m$, which means $100m$ in front of the robot, and the system still have a dumb result.

---

# Adding torque disturbance experiment

Here we introduce torque disturbance. We subtract the value of torque output from motor $\tau$ with some value $\tau_d$. The value of torque disturbance is relative to motor angular velocity. If the motor is stopping $\omega=0$ then there is no disturbance $\tau_d=0$. Because a constant value of disturbance will add a backward movement to the robot when it should not be. The direction of disturbance is the opposite of the movement, just like friction force. This sentences can be described in this formula:
$$
\tau_d = \text{sign}(\omega)*G
$$
With $G$ is value of the disturbance. The value is constant except the direction. In short, $\tau_d$ is ranging to $\{-G,0,G\}$.
The concept seems logical but I've got no results because the simulation get really slow. The time sampling of simulation turns to $3\cdot10^{-27}$s

---

# Adding force disturbance experiment (friction force)

We got a friction model from MATLAB which can be found in Ref. [[4]](https://www.mathworks.com/help/simulink/slref/friction-model-with-hard-stops.html). This is the preview picture of that model.
Basically this model will put a spring force. The spring force is given by initial length of spring $x$. Also because of this force, the $x$ is changed each second. But each iteration, the force is reduced because of the friction and so $x$ is reduced each iteration.

----

What we adopt from this model is the friction force model.
We modify the model so we can input the force from the wheel torque and the system will output a reduced force, by friction. Here also we use a Normal Force from our model, that is the mass of the robot multiplied by gravity ($F_N=W=Mg$). So, we got an input force, and output force as indicated by cyan mark in above picture.

----

We put the model in our system like this picture below.
We used pretty big friction coefficient of $\mu_s=3.07$ (the static coefficient of friction, SCOF) and $\mu_d=2.8$ (the dynamic coefficient of friction, DCOF)
Here is the result: (like always, open image in new tab if you want it bigger)
Here we can see that, even using a big friction force, the system cannot reach the goal properly.
So **We revoke our hypothesis that it is either the system or the simulation that is bad**. Maybe, this fuzzy system is designed for a robot with a high torque motor or a heavy robot so that it may stop on the goal position.
*Knowing this problem, we would like to try a fuzzy controller that allows a backward movement. So the system may reduce the speed by moving backward. It may hold its position on the goal point by balancing backward and forward movement*.

---

# Yet another parameter experiment (High torque motor)

So here we go back to our default model. Our model in the beginning of this note. We don't add any disturbance, but rather we try to use another motor and robot parameters.
Here is our code:

%% Parameters for C23-L33-W20 terminal voltage for this motor is 12V
La= 0.94e-3; %H, inductance of the armature winding
Ra= 1; %Ohm, resistance of the armature winding
Kb=  0.0301; %V/rad/s the back emf constant 

N=10;  %the gear ratio
Kt=0.03; %Nm/Amp the torque constant

%% Parameters for robot bod
M = 1000e-3; % Robot mass in KG
d = 0.0; % Location of center of gravity of robot. x=d y=o in robot frame
R = 0.02;%radius of wheel in meter
L = 20e-2;%distance between wheel in meter
J = 0.01;%  Kg*m^2  Moment inertia of disk robot KG*m^2 calculated using https://www.omnicalculator.com/physics/mass-moment-of-inertia


%% Input
V_r = 1; % right motor voltage
V_l = 0; % right motor voltage

G_x = -10; %Goal x position
G_y = 0; %Goal y position
G_g = pi/2; %Goal z orientation

xa=0;
ya=0;
theta=0;

vol_gain = 1; % to slowdown the robot

TF = 5; %simulation time

mu_s = 0;%1.07;
mu_k = 0;%0.953;

%% Run simulation
simout = sim('func1_v4');
figure(1);
subplot(3,3,1),plot(simout.tout, simout.ya), xlabel('t(s)'), ylabel('ya(m)');
subplot(3,3,2),plot(simout.tout, simout.xa), xlabel('t(s)'), ylabel('xa(m)');
subplot(3,3,3),plot(simout.xa, simout.ya, G_x, G_y, '*'), xlabel('xa(m)'), ylabel('ya(m)');
subplot(3,3,4),plot(simout.tout, simout.theta), xlabel('t(s)'), ylabel('theta(rad)');
subplot(3,3,5),plot(simout.tout, simout.poser), xlabel('t(s)'), ylabel('position error(m)');
subplot(3,3,6),plot(simout.tout, simout.orrer), xlabel('t(s)'), ylabel('heading error(deg)');
subplot(3,3,7),plot(simout.tout, simout.vr_flc), xlabel('t(s)'), ylabel('right motor armature (volt)');
subplot(3,3,8),plot(simout.tout, simout.vl_flc), xlabel('t(s)'), ylabel('left motor armature (volt)');
subplot(3,3,9),plot(simout.tout, simout.vl_flc, 'r', simout.tout, simout.vr_flc, 'b'), xlabel('t(s)'), ylabel('armature (volt)');

We use the same motor model (C23-L33-W20) from Ref.[3]. It is the most realistic because the terminal voltage to operate this motor is $12V$. But there is difference in inductance value than the previous model. Now we use $La= 0.94\cdot10^{-3}H$. Previously we use $La= 0.94H$. It is pretty extreme difference. The other parameter we changed is the moment of inertia. So we assume the robot is a disk with the center of the mass in its center. Robot mass is $1Kg$ with the radius of $20cm$. Using a tool from Ref. [[6]](https://www.omnicalculator.com/physics/mass-moment-of-inertia), we can roughly estimate the mass moment inertia of the robot as $J=0.01\ Kg\ m^2$. The most important part is the gear ratio parameter. We chose gear ratio $N=10$ to simulate a high torque motor. Higher $N$ will produce higher torque ($\tau=NK_ti_a$).

----

Here is when we place the goal to $(-10,0)$. Let's see if the robot can reach the goal which is $10m$ behind it. Using this parameter, the result is pretty good. The robot reach distance of $10m$ in 3 seconds or with a speed of $10.8km/s$. For comparison, human walk roughly with speed of 5km/h, or about $1.4m/s$. So, we conclude that the result seems logical or realistic. We can continue research on using this model.

----

# Conclusion

The designed system is good enough. We just need a right parameter to make a simulation that is close to real life results.The controller is good but still we would like to know the result if we allows backward movement, so the robot will not miss the goal. For the future work, we can use the last parameter combined with a better controller and see if we got better results.

---

# Trying another controller

----

## Heading error fuzzification

We use $9$ classes. Error is calculated in degree angles.

----

## Distance error fuzzification
We use $11$ classes. Error is calculated in the unit of meters.

----

## Motor voltage fuzzification
We use 11 classes. The output voltage is in the unit of volts.

----

## Rules

----

## Code

function [v_r,v_l] = tflc(position_error,orientation_error)
    %% global constant
    %TFLC RULES
    r_w_v = [                   %Rules for right wheel
        1 1 1 1 1 2 2 3 4;
        1 1 1 2 2 3 3 4 5;
        1 1 2 2 3 4 4 5 6;
        1 1 2 3 4 5 5 5 7;
        1 2 3 4 5 6 6 6 8;
        1 2 3 4 6 7 7 7 9;
        1 2 4 5 7 8 8 9 10;
        1 3 5 6 8 9 9 10 11;
        1 4 5 6 9 10 10 11 11;
        1 4 6 7 10 10 11 11 11;
        1 4 6 8 11 11 11 11 11;
        ];
    l_w_v = flip(r_w_v,2);      %Rules for left wheel

    %% run all nested function
    %fuzzyfication
    fuz_dist = check_distance_membership(position_error); % in meter
    fuz_orie = check_err_angle_membership(orientation_error); % in degree angle
    %inference
    [v_TFLC_R_th,v_TFLC_L_th]=relate_dis_head(fuz_dist, fuz_orie, r_w_v, l_w_v);
    %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
    v_r = centeroid_TFLC_R;
    v_l = centeroid_TFLC_L;
    
    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%                      FUNCTIONS
    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% =====================to check all value in distance fuzzification
    function out = check_distance_membership(input) %meter
        d0 = triangle (input, 0.0, 0.0, 0.3, 1, 1, 0); %scale 0
        d1 = triangle (input, 0.0, 0.3, 0.6, 0, 1, 0); %scale 1
        d2 = triangle (input, 0.3, 0.6, 0.9, 0, 1, 0); %scale 2
        d3 = triangle (input, 0.6, 0.9, 1.2, 0, 1, 0); %scale 3
        d4 = triangle (input, 0.9, 1.2, 1.5, 0, 1, 0); %scale 4
        d5 = triangle (input, 1.2, 1.5, 1.8, 0, 1, 0); %scale 5
        d6 = triangle (input, 1.5, 1.8, 2.1, 0, 1, 0); %scale 6
        d7 = triangle (input, 1.8, 2.1, 2.4, 0, 1, 0); %scale 7
        d8 = triangle (input, 2.1, 2.4, 2.7, 0, 1, 0); %scale 8
        d9 = triangle (input, 2.4, 2.7, 3.0, 0, 1, 0); %scale 9
        d10 = triangle(input, 2.7, 3.0, 3.0, 0, 1, 1); %scale 10
        out = [d0;d1;d2;d3;d4;d5;d6;d7;d8;d9;d10];
    end
    %% =====================to check all value in angle error fuzzification
    function out = check_err_angle_membership(input)
        nl = triangle (input, -180, -160, -120, 1, 1, 0); %negative large
        nm = triangle (input, -160, -120, -80, 0, 1, 0); %negative medium
        ns = triangle (input, -120, -80, -40, 0, 1, 0); %negative small
        n = triangle (input, -80, -40, 0, 0, 1, 0);     %negative
        z = triangle (input, -40, 0, 40, 0, 1, 0);      %zero
        p = triangle (input, 0, 40, 80, 0, 1, 0);       %positive
        ps = triangle (input, 40, 80, 120, 0, 1, 0);    %positive small
        pm = triangle (input, 80, 120, 160, 0, 1, 0);   %positive medium
        pl = triangle (input, 120, 160, 180, 0, 1, 1);  %positive large
        out = [nl; nm; ns; n; z; p; ps; pm; pl];
    end
    %% =====================to check all value in velocity of motors in TFLC fuzzification
    function out = check_velocity_membership(input)
        v0 = triangle (input, 0, 0, 1, 1, 1, 0);%zero
        v1 = triangle (input, 0, 1, 2, 0, 1, 0);%slow
        v2 = triangle (input, 1, 2, 3, 0, 1, 0);%near medium
        v3 = triangle (input, 2, 3, 4, 0, 1, 0);%medium
        v4 = triangle (input, 3, 4, 5, 0, 1, 0);%near high
        v5 = triangle (input, 4, 5, 6, 0, 1, 0);%high
        v6 = triangle (input, 5, 6, 7, 0, 1, 0);%very high
        v7 = triangle (input, 6, 7, 8, 0, 1, 0);%very high
        v8 = triangle (input, 7, 8, 9, 0, 1, 0);%very high
        v9 = triangle (input, 8, 9, 11, 0, 1, 0);%very high
        v10 = triangle (input, 9, 10, 12, 0, 1, 1);%very high
        out = [v0;v1;v2;v3;v4;v5;v6;v7;v8;v9;v10];
    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;
        else
            out = 0;
        end
    end

    %% =Relate one fuzzification to another: matrix & vec======
    % the relation is using minimum, because the relation in fuzzy rules are
    % using AND
    function [v_out_R, v_out_L] = relate_dis_head(distance, heading, rules_R, rules_L)
        % set1 is rows, and set2 is columns
        % find correspoding values for each rules
        m = zeros(length(distance), length(heading));
        for i=1:length(distance)
            for j=1:length(heading)
                m(i,j)=min(distance(i),heading(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_R = zeros(max(max(rules_R)),1);
        v_out_L = v_out_R;
        [w,h]=size(rules_R);
        for i = 1:w
            for j = 1:h
                v_out_R( rules_R(i,j) ) = max(v_out_R( rules_R(i,j) ), m (i,j)); %MAX
                v_out_L( rules_L(i,j) ) = max(v_out_L( rules_L(i,j) ), m (i,j)); %MAX
            end
        end
    end
end

----

## Result

This is the result when the goal posiiton is $10m$ behind the robot.The control is faster, and it can stop at the goal position.

----

And this is the plot of speed, position, and heading over time period. The result is better than before.

----

This is the result when the goal is $10m$ around the robot with various angles.

---

# Trying another controller 2

## Modify angle classes

## Rules modification

## Code

function [v_r,v_l] = tflc(position_error,orientation_error)
    %% global constant
    %TFLC RULES
    r_w_v = [                   %Rules for right wheel
            1	1	1	1	1	1	2	3	4	5	6;
            1	1	1	1	1	2	3	4	5	6	7;
            1	1	1	1	2	3	4	5	6	7	8;
            1	1	1	2	3	4	5	6	7	8	9;
            1	1	2	3	4	5	6	7	8	9	10;
            1	2	3	4	5	6	7	8	9	10	11;
            1	1	2	3	5	7	8	9	10	11	11;
            1	1	1	4	6	8	9	10	11	11	11;
            1	1	1	1	7	9	10	11	11	11	11;
            1	1	1	1	1	10	11	11	11	11	11;
            1	1	1	1	1	11	11	11	11	11	11;
        ];
    l_w_v = flip(r_w_v,2);      %Rules for left wheel

    %% run all nested function
    %fuzzyfication
    fuz_dist = check_distance_membership(position_error); % in meter
    fuz_orie = check_err_angle_membership(orientation_error); % in degree angle
    %inference
    [v_TFLC_R_th,v_TFLC_L_th]=relate_dis_head(fuz_dist, fuz_orie, r_w_v, l_w_v);
    %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
    v_r = centeroid_TFLC_R;
    v_l = centeroid_TFLC_L;
    
    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%                      FUNCTIONS
    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% =====================to check all value in distance fuzzification
    function out = check_distance_membership(input) %meter
        d0 = triangle (input, 0.0, 0.0, 0.3, 1, 1, 0); %scale 0
        d1 = triangle (input, 0.0, 0.3, 0.6, 0, 1, 0); %scale 1
        d2 = triangle (input, 0.3, 0.6, 0.9, 0, 1, 0); %scale 2
        d3 = triangle (input, 0.6, 0.9, 1.2, 0, 1, 0); %scale 3
        d4 = triangle (input, 0.9, 1.2, 1.5, 0, 1, 0); %scale 4
        d5 = triangle (input, 1.2, 1.5, 1.8, 0, 1, 0); %scale 5
        d6 = triangle (input, 1.5, 1.8, 2.1, 0, 1, 0); %scale 6
        d7 = triangle (input, 1.8, 2.1, 2.4, 0, 1, 0); %scale 7
        d8 = triangle (input, 2.1, 2.4, 2.7, 0, 1, 0); %scale 8
        d9 = triangle (input, 2.4, 2.7, 3.0, 0, 1, 0); %scale 9
        d10 = triangle(input, 2.7, 3.0, 3.0, 0, 1, 1); %scale 10
        out = [d0;d1;d2;d3;d4;d5;d6;d7;d8;d9;d10];
    end
    %% =====================to check all value in angle error fuzzification
    function out = check_err_angle_membership(input)
        nxl = triangle (input, -180, -150, -120, 1, 1, 0); %negative large
        nl = triangle (input, -150, -120, -90, 0, 1, 0); %negative large
        nm = triangle (input, -120, -90, -60, 0, 1, 0); %negative medium
        ns = triangle (input, -90, -60, -30, 0, 1, 0); %negative small
        n = triangle (input, -60, -30, 0, 0, 1, 0);     %negative
        z = triangle (input, -30, 0, 30, 0, 1, 0);      %zero
        p = triangle (input, 0, 30, 60, 0, 1, 0);       %positive
        ps = triangle (input, 30, 60, 90, 0, 1, 0);    %positive small
        pm = triangle (input, 60, 90, 120, 0, 1, 0);   %positive medium
        pl = triangle (input, 90, 120, 150, 0, 1, 0);  %positive large
        pxl = triangle (input, 120, 150, 180, 0, 1, 1);  %positive large
        out = [nxl; nl; nm; ns; n; z; p; ps; pm; pl; pxl];
    end
    %% =====================to check all value in velocity of motors in TFLC fuzzification
    function out = check_velocity_membership(input)
        v0 = triangle (input, 0, 0, 1, 1, 1, 0);%zero
        v1 = triangle (input, 0, 1, 2, 0, 1, 0);%slow
        v2 = triangle (input, 1, 2, 3, 0, 1, 0);%near medium
        v3 = triangle (input, 2, 3, 4, 0, 1, 0);%medium
        v4 = triangle (input, 3, 4, 5, 0, 1, 0);%near high
        v5 = triangle (input, 4, 5, 6, 0, 1, 0);%high
        v6 = triangle (input, 5, 6, 7, 0, 1, 0);%very high
        v7 = triangle (input, 6, 7, 8, 0, 1, 0);%very high
        v8 = triangle (input, 7, 8, 9, 0, 1, 0);%very high
        v9 = triangle (input, 8, 9, 11, 0, 1, 0);%very high
        v10 = triangle (input, 9, 10, 12, 0, 1, 1);%very high
        out = [v0;v1;v2;v3;v4;v5;v6;v7;v8;v9;v10];
    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;
        else
            out = 0;
        end
    end

    %% =Relate one fuzzification to another: matrix & vec======
    % the relation is using minimum, because the relation in fuzzy rules are
    % using AND
    function [v_out_R, v_out_L] = relate_dis_head(distance, heading, rules_R, rules_L)
        % set1 is rows, and set2 is columns
        % find correspoding values for each rules
        m = zeros(length(distance), length(heading));
        for i=1:length(distance)
            for j=1:length(heading)
                m(i,j)=min(distance(i),heading(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_R = zeros(max(max(rules_R)),1);
        v_out_L = v_out_R;
        [w,h]=size(rules_R);
        for i = 1:w
            for j = 1:h
                v_out_R( rules_R(i,j) ) = max(v_out_R( rules_R(i,j) ), m (i,j)); %MAX
                v_out_L( rules_L(i,j) ) = max(v_out_L( rules_L(i,j) ), m (i,j)); %MAX
            end
        end
    end
end

## Results


From the result we can see that the system is capable of faster heading correction!
Not only it can maneuver sharply from far away, but also from close distance.
This is beacuse we introduce a turning only region in the fuzzy rules.

Here is the rules for right motor output voltage. The yellow marked region is the region where the robot will only turning, without moving forward. It is either when the robot is too far from the goal position or too close to the goal position.

# Path tracking upgrade

In this part we add algorithm for the robot to follow point array. This sceme is similar to Line follower robot.
The robot will approach one of the points. When the distance of the robot to that point is bellow 0.1 m $(position error < 0.1m)$, the robot will approch the next point.

Block diagram:
The changes that I made are `path_iterator` function and `robot_path` constant variable. `robot_path` is array containing points. In this case, the points resemble figure of 8 (*the lemniscate of Gerono, or lemniscate of Huygens*). The job of `path_iterator` is to feed 1 point (x and y coordinate) to the system to approach. When the robot succesfully approach that point, `path_iterator` will feed another point.
Code:

function [i_next,x,y] = path_iterator(i, pos_err, robot_path)
    [r,c] = size(robot_path);    
    if pos_err < 0.1
        i = i + 1;
        if i > c
            i = 1;
        end
    end
    
    x = robot_path(1,i);
    y = robot_path(2,i);
    i_next = i;
    
end

Because there is an iterative variable in that block, we need to give an initial value to that variable. We do by putting *Memory block*  from HDL/Discrete library. We give both block initial value of $1$.

To create the robot path, I made a routine in the matlab main code:

%% the lemniscate of Gerono, or lemniscate of Huygens, or figure-eight curve
t = 0:0.02: 2*pi; % t is between 1~2pi
x = cos(t);
y = sin(2*t)/2;

plot(x,y);

robot_path = [x;y];

That code will give you a figure of 8 array.

And this is the code in `path_iterator` function:

function [i_next,x,y] = path_iterator(i, pos_err, robot_path)
    [r,c] = size(robot_path);    
    if pos_err < 0.1
        i = i + 1;
        if i > c
            i = 1;
        end
    end
    
    x = robot_path(1,i);
    y = robot_path(2,i);
    i_next = i;
    
end
%(i,pos_err) are initially 1 from memory block 

This is the result of how the robot approach that points:


# 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.
[2] Dhaouadi, Rached, and A. Abu Hatab. "Dynamic modelling of differential-drive mobile robots using lagrange and newton-euler methodologies: A unified framework." Advances in Robotics & Automation 2.2 (2013): 1-7.
[3] [Moog Components Group, "Permanent Magnet DC Motors datasheet"](https://www.moog.com/content/dam/moog/literature/MCG/moc23series.pdf)
[4] [MATLAB friction model](https://www.mathworks.com/help/simulink/slref/friction-model-with-hard-stops.html)
[5] Li, Kai Way, et al. "Floor slipperiness measurement: friction coefficient, roughness of floors, and subjective perception under spillage conditions." Safety Science 42.6 (2004): 547-565.
[6] [Mass Moment of Inertia Calculator](https://www.omnicalculator.com/physics/mass-moment-of-inertia)

---

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

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