fc.rar_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) ou