a fuzzy controller for an inverted pendulum.c

a fuzzy controller for an inverted pendulum

/*
  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)
          out_index = -2;
        else if (out_index > 2)
          out_index = 2;

        /* Shift indices right.*/
        out_index += 2;

        /* Determine the certainty of the premise */
        outdom = MIN((emem->dom[pos[0]+i]), (edotmem->dom[pos[1]+j]));

        /* Defuzzify */
        area = 2*outmem->width*(outdom - (outdom*outdom)/2);
        Atot += area;
        WAtot += area*outmem->center[out_index];
      }
    }
  }
  /* Return the crisp value.  Minus sign required to give correct output for 
     pendulum system!  Note that this minus sign actually ensures that the table of
     indices works out as shown in class. */

  return -(WAtot/Atot); 
}
 
/************************************************************************************/ 

#if 0  /* Set to zero to call fuzzy_control() from another program. */ 

/* Test I/O behavior of fuzzy controller. */

void main(void) {

/* Test for input given below.  Output is -6.818182. */

  double e, edot, u;
  FUZ_SYS fuzzy_system;

  /* Crips inputs. */
  e = 0;                 
  edot = PI/8.0 - PI/32;

  fuzzy_init(&fuzzy_system); 
  u = fuzzy_control(e,edot,&fuzzy_system);
  fuzzy_free(&fuzzy_system);
  printf("e = %f, edot = %f, u = %f\n",e,edot,u); 
}
#endif

/************************************************************************************/