/*
Program: fuzzy.c
Written by: Scott Brown
2 input fuzzy controller to control inverted pendulum system. Controller has
5 membship functions for each input and 5 membership functions for the output.
Center-of-gravity is used for defuzzification.
*/
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#define MAX(A,B) ((A) > (B) ? (A) : (B))
#define MIN(A,B) ((A) < (B) ? (A) : (B))
#define PI 3.14159265359
/************************************************************************************/
typedef struct in_mem {
double width; /* Input membership function width (1/2 of triangle base). */
double *center; /* Center of each input membership function. */
double *dom; /* Degree of membership for each membership function. */
} IN_MEM;
typedef struct out_mem {
double width; /* Output membership function width (1/2 of triangle base). */
double *center; /* Center of each output membership function. */
} OUT_MEM;
typedef struct fuz_sys {
IN_MEM *emem; /* Groups all fuzzy system parameters in a single variable. */
IN_MEM *edotmem;
OUT_MEM *outmem;
} FUZ_SYS;
/************************************************************************************/
/* Function Prototypes: */
void fuzzy_init(FUZ_SYS *fuzzy_system);
void fuzzy_free(FUZ_SYS *fuzzy_system);
double fuzzy_control(double e, double edot, FUZ_SYS *fuzzy_system);
void fuzzyify(double u, IN_MEM *mem);
double leftall(double u, double w, double c);
double rightall(double u, double w, double c);
double triangle(double u, double w, double c);
void match(const IN_MEM *emem, const IN_MEM *edotmem, int *pos);
double inf_defuzz(IN_MEM *emem, IN_MEM *edotmem, OUT_MEM *outmem, int *pos);
/************************************************************************************/
void fuzzy_init(FUZ_SYS *fuzzy_system) {
/* Define the input and output membership functions. */
int i;
/* Allocate memory for membership functions. */
if (!(fuzzy_system->emem = (IN_MEM *) malloc(sizeof(IN_MEM)))) {
printf("Error allocating memory.\n");
exit(1);
}
if (!(fuzzy_system->edotmem = (IN_MEM *) malloc(sizeof(IN_MEM)))) {
printf("Error allocating memory.\n");
exit(1);
}
if (!(fuzzy_system->outmem = (OUT_MEM *) malloc(sizeof(OUT_MEM)))) {
printf("Error allocating memory.\n");
exit(1);
}
if (!(fuzzy_system->emem->center = (double *) malloc(5*sizeof(double)))) {
printf("Error allocating memory.\n");
exit(1);
}
if (!(fuzzy_system->emem->dom = (double *) malloc(5*sizeof(double)))) {
printf("Error allocating memory.\n");
exit(1);
}
if (!(fuzzy_system->edotmem->center = (double *) malloc(5*sizeof(double)))) {
printf("Error allocating memory.\n");
exit(1);
}
if (!(fuzzy_system->edotmem->dom = (double *) malloc(5*sizeof(double)))) {
printf("Error allocating memory.\n");
exit(1);
}
if (!(fuzzy_system->outmem->center = (double *) malloc(5*sizeof(double)))) {
printf("Error allocating memory.\n");
exit(1);
}
/* Initialize for inverted pendulum. */
fuzzy_system->emem->width = PI/4.0; /* Width defined to be 1/2 of triangle base. */
fuzzy_system->edotmem->width = PI/8.0;
fuzzy_system->outmem->width = 10;
for (i=0; i<5; i++) {
fuzzy_system->emem->center[i] = (-PI/2.0 + i*PI/4.0);
fuzzy_system->edotmem->center[i] = (-PI/4.0 + i*PI/8.0);
fuzzy_system->outmem->center[i] = (-20.0 + i*10.0);
}
}
/************************************************************************************/
void fuzzy_free(FUZ_SYS *fuzzy_system) {
/* Free memory allocated in fuzzy_init(). */
free(fuzzy_system->emem->center);
free(fuzzy_system->emem->dom);
free(fuzzy_system->edotmem->center);
free(fuzzy_system->edotmem->dom);
free(fuzzy_system->outmem->center);
free(fuzzy_system->emem);
free(fuzzy_system->edotmem);
free(fuzzy_system->outmem);
}
/************************************************************************************/
double fuzzy_control(double e, double edot, FUZ_SYS *fuzzy_system) {
/* Given crisp inputs e and edot, determine the crisp output u. */
int pos[2];
fuzzyify(e, fuzzy_system->emem);
fuzzyify(edot, fuzzy_system->edotmem);
match(fuzzy_system->emem, fuzzy_system->edotmem, pos);
return inf_defuzz(fuzzy_system->emem, fuzzy_system->edotmem, fuzzy_system->outmem, pos);
}
/************************************************************************************/
void fuzzyify(double u, IN_MEM *mem) {
/* Fuzzify the input u by determining the degree of membership for each membership
function in mem. Assumes 5 membership functions, with first and last membership
functions leftall and rightall respectively. Other membership functions are
triangular. */
int i;
mem->dom[0] = leftall(u, mem->width, mem->center[0]);
for (i=1; i<4; i++)
mem->dom[i] = triangle(u, mem->width, mem->center[i]);
mem->dom[4] = rightall(u, mem->width, mem->center[4]);
}
/************************************************************************************/
double leftall(double u, double w, double c)
/* Determine degree of membership for a leftall membership function.
NOTE: u is input, c is mem. fun. center, and w is mem. fun. width. */
{
if (u < c)
return 1.0;
else
return MAX(0,(1-(u-c)/w));
}
/************************************************************************************/
double rightall(double u, double w, double c)
/* Determine degree of membership for a RIGHTALL membership function
NOTE: u is input, c is mem. fun. center, and w is mem. fun. width. */
{
if (u >= c)
return 1.0;
else
return MAX(0,(1-(c-u)/w));
}
/************************************************************************************/
double triangle(double u, double w, double c)
/* Determine degree of membership for a TRIANGLE membership function
NOTE: u is input, c is mem. fun. center, and w is mem. fun. width. */
{
if (u >= c)
return MAX(0,(1-(u-c)/w));
else
return MAX(0,(1-(c-u)/w));
}
/************************************************************************************/
void match(const IN_MEM *emem, const IN_MEM *edotmem, int *pos) {
/* For each universe of discourse, determine the index of the first membership function
with a non-zero degree (i.e. match the rules to the current inputs to find which rules
are on). These indices are used to determine which four rules to evaluate. (NOTE:
A 2 input sytem with no more than 50% overlap for input membership functions only
requires the evaluation of at most 4 rules.) */
int i;
for (i=0; i<5; i++) {
if(emem->dom[i] != 0) {
pos[0] = i;
break;
}
}
for (i=0; i<5; i++) {
if(edotmem->dom[i] != 0) {
pos[1] = i;
break;
}
}
}
/************************************************************************************/
double inf_defuzz(IN_MEM *emem, IN_MEM *edotmem, OUT_MEM *outmem, int *pos) {
/* We use the degrees of membership found in the function match() to form the implied
fuzzy sets. The correct output membership function for each rule is determined by
adding (and saturating) a shifted version of the input membership function indices
(this implements the typical pattern of linguistic-numeric indices in the body of
the table of rules). In this way we compute the rule-base at every step, rather
than storing the rule-base in a table. Defuzzification is also performed using
the center-of-gravity method. A crisp output is returned. */
double outdom, area, Atot = 0, WAtot = 0;
int i, j, out_index;
for(i=0; i<2; i++) {
for(j=0; j<2; j++) {
if ( ((pos[0]+i)<5) && ((pos[1]+j)<5)) { /* Check that bounds are not exceeded. */
outdom = 0;
/* Shift indices left. */
out_index = ((pos[0]+i)-2) + ((pos[1]+j)-2);
/* Saturate */
if (out_index < -2)
ou