%
% EE552 Fuzzy Controller Simulation
%
%
% Try to keep 2 velocities to be the same, this is done by controlling
% the difference to be 0
%

% Measure the milliseconds
clear all;
N = 1000;
time = linspace( 0, 1000, N );

% Pick starting points
velocity_0 = 150;
velocity_1 = 75;

% Initialize the delta to 0
old_difference = 0;

% Initialize the fuzzy rules
difference_rules = [ 23, 7, 1, 0, -1, -7, -23]; % PL PM PS Z NS NM NL
delta_rules      = [ 23, 7, 1, 0, -1, -7, -23]; % PL PM PS Z NS NM NL
output_rules     = [ 23, 7, 1, 0, -1, -7, -23]; % PL PM PS Z NS NM NL

% Define the fuzzy terms
plarge  = 1;
pmedium = 2;
psmall  = 3;
zero    = 4;
nsmall  = 5;
nmedium = 6;
nlarge  = 7;

for i = 1:length( time )

  % Calculate control variables
  difference(i)  = velocity_0 - velocity_1;
  delta          = difference(i) - old_difference;
  old_difference = difference(i);

  % Fuzzify the inputs
  fdiff  = fuzzification( difference(i), difference_rules );
  fdelta = fuzzification( delta, delta_rules );

  if     fdiff == plarge & fdelta == plarge

    output_0 = nlarge;
    output_1 = plarge;
 
  elseif fdiff == plarge & fdelta == pmedium

    output_0 = nlarge;
    output_1 = plarge;
 
  elseif fdiff == plarge & fdelta == psmall

    output_0 = nlarge;
    output_1 = plarge;
 
  elseif fdiff == plarge & fdelta == zero

    output_0 = nlarge;
    output_1 = plarge;
 
  elseif fdiff == plarge & fdelta == nsmall

    output_0 = nlarge;
    output_1 = plarge;
 
  elseif fdiff == plarge & fdelta == nmedium

    output_0 = nmedium;
    output_1 = pmedium;
 
  elseif fdiff == plarge & fdelta == nlarge

    output_0 = nmedium;
    output_1 = pmedium;
 
  elseif fdiff == pmedium & fdelta == plarge

    output_0 = nlarge;
    output_1 = plarge;
 
  elseif fdiff == pmedium & fdelta == pmedium

    output_0 = nlarge;
    output_1 = plarge;
 
  elseif fdiff == pmedium & fdelta == psmall

    output_0 = nmedium;
    output_1 = pmedium;
 
  elseif fdiff == pmedium & fdelta == zero

    output_0 = nmedium;
    output_1 = pmedium;
 
  elseif fdiff == pmedium & fdelta == nsmall

    output_0 = nmedium;
    output_1 = pmedium;
 
  elseif fdiff == pmedium & fdelta == nmedium

    output_0 = nsmall;
    output_1 = psmall;
 
  elseif fdiff == pmedium & fdelta == nlarge

    output_0 = nsmall;
    output_1 = psmall;
 
  elseif fdiff == psmall & fdelta == plarge

    output_0 = nsmall;
    output_1 = psmall;
 
  elseif fdiff == psmall & fdelta == pmedium

    output_0 = nsmall;
    output_1 = psmall;
 
  elseif fdiff == psmall & fdelta == psmall

    output_0 = nsmall;
    output_1 = psmall;
 
  elseif fdiff == psmall & fdelta == zero

    output_0 = nsmall;
    output_1 = zero;
 
  elseif fdiff == psmall & fdelta == nsmall

    output_0 = nsmall;
    output_1 = zero;
 
  elseif fdiff == psmall & fdelta == nmedium

    output_0 = zero;
    output_1 = psmall;
 
  elseif fdiff == psmall & fdelta == nlarge

    output_0 = zero;
    output_1 = psmall;
 
  elseif fdiff == zero & fdelta == plarge

    output_0 = zero;
    output_1 = zero;
 
  elseif fdiff == zero & fdelta == pmedium

    output_0 = zero;
    output_1 = zero;
 
  elseif fdiff == zero & fdelta == psmall

    output_0 = zero;
    output_1 = zero;
 
  elseif fdiff == zero & fdelta == zero

    output_0 = zero;
    output_1 = zero;
 
  elseif fdiff == zero & fdelta == nsmall

    output_0 = zero;
    output_1 = zero;
 
  elseif fdiff == zero & fdelta == nmedium

    output_0 = zero;
    output_1 = zero;
 
  elseif fdiff == zero & fdelta == nlarge

    output_0 = zero;
    output_1 = zero;
 
  elseif fdiff == nsmall & fdelta == plarge

    output_0 = psmall;
    output_1 = nsmall;
 
  elseif fdiff == nsmall & fdelta == pmedium

    output_0 = psmall;
    output_1 = nsmall;
 
  elseif fdiff == nsmall & fdelta == psmall

    output_0 = psmall;
    output_1 = zero;
 
  elseif fdiff == nsmall & fdelta == zero

    output_0 = psmall;
    output_1 = zero;
 
  elseif fdiff == nsmall & fdelta == nsmall

    output_0 = zero;
    output_1 = nsmall;
 
  elseif fdiff == nsmall & fdelta == nmedium

    output_0 = zero;
    output_1 = nsmall;
 
  elseif fdiff == nsmall & fdelta == nlarge

    output_0 = zero;
    output_1 = nsmall;
 
  elseif fdiff == nmedium & fdelta == plarge

    output_0 = plarge;
    output_1 = nlarge;
 
  elseif fdiff == nmedium & fdelta == pmedium

    output_0 = plarge;
    output_1 = nlarge;
 
  elseif fdiff == nmedium & fdelta == psmall

    output_0 = pmedium;
    output_1 = nmedium;
 
  elseif fdiff == nmedium & fdelta == zero

    output_0 = pmedium;
    output_1 = nmedium;
 
  elseif fdiff == nmedium & fdelta == nsmall

    output_0 = pmedium;
    output_1 = nmedium;
 
  elseif fdiff == nmedium & fdelta == nmedium

    output_0 = psmall;
    output_1 = nsmall;
 
  elseif fdiff == nmedium & fdelta == nlarge

    output_0 = psmall;
    output_1 = nsmall;
 
  elseif fdiff == nlarge & fdelta == plarge

    output_0 = plarge;
    output_1 = nlarge;
 
  elseif fdiff == nlarge & fdelta == pmedium

    output_0 = plarge;
    output_1 = nlarge;
 
  elseif fdiff == nlarge & fdelta == psmall

    output_0 = plarge;
    output_1 = nlarge;
 
  elseif fdiff == nlarge & fdelta == zero

    output_0 = plarge;
    output_1 = nlarge;
 
  elseif fdiff == nlarge & fdelta == nsmall

    output_0 = plarge;
    output_1 = nlarge;
 
  elseif fdiff == nlarge & fdelta == nmedium

    output_0 = pmedium;
    output_1 = nmedium;
 
  elseif fdiff == nlarge & fdelta == nlarge

    output_0 = pmedium;
    output_1 = nmedium;

  end;

  velocity_0 = velocity_0 + defuzzification( output_0, output_rules );
  velocity_1 = velocity_1 + defuzzification( output_1, output_rules );

end;

figure(1);
plot( time, difference );
title( 'Control of difference to 0' );
axis( [0 25 -75 75] );

difference_rules = [ 23, 7, 1, 0, -1, -7, -23]; % PL PM PS Z NS NM NL

x = linspace( -50, 50, 1000 );

for i = 1 : length( x )

  if x(i) >= difference_rules( plarge )
    pl(i) = 1;
  else
    pl(i) = 0;
  end;

  if x(i) >= difference_rules( pmedium ) & x(i) < difference_rules( plarge )
    pm(i) = 1;
  else
    pm(i) = 0;
  end;

  if x(i) >= difference_rules( psmall ) & x(i) < difference_rules( pmedium )
    ps(i) = 1;
  else
    ps(i) = 0;
  end;

  if x(i) == difference_rules( zero )
    z(i) = 1;
  else
    z(i) = 0;
  end;

  if x(i) <= difference_rules( nsmall ) & x(i) > difference_rules( nmedium )
    ns(i) = 1;
  else
    ns(i) = 0;
  end;

  if x(i) <= difference_rules( nmedium ) & x(i) > difference_rules( nlarge )
    nm(i) = 1;
  else
    nm(i) = 0;
  end;

  if x(i) <= difference_rules( nlarge )
    nl(i) = 1;
  else
    nl(i) = 0;
  end;

end;

figure(2);
%hold on;
plot( x,nl,x,nm,x,ns,x,z,x,ps,x,pm,x,pl );
title( 'Membership Functions' );
axis( [-50 50 -0.5 1.5] );
