/* General-purpose fuzzy inference engine supporting any number of system
inputs and outputs, membership functions, and rules. Membership functions can
be any shape defineable by 2 points and 2 slopes--trapezoids, triangles,
rectanlges, etc. Rules can have any number of antecedents and outputs, and can
vary from rule to rule. "Min" method is used to compute rule strength, "Max"
for applying rule strengths, "Center-of-Gravity" for defuzzification. This
implementation of Inverted Pendulum control problem has: System Inputs, 2
(pendulum angle and velocity); System Outputs, 1 (force supplied to base of
pendulum); Membership Functions, 7 per system input/output; Rules, 15 (each
with 2 antecedents & 1 output). If more precision is required, integers can
be changed to real numbers.*/
#include <stdio.h>
#define MAXNAME 10 /* max number of characters in names */
#define UPPER_LIMIT 255 /* max number assigned as degree of membership */
void PID();
/* io_type structure builds a list of system inputs and a list of system
outputs. After initialization, these lists are fixed, except for value field
which is updated on every inference pass. */
struct io_type{
char name[MAXNAME]; /* name of system input/output */
int value; /* value of system input/output */
struct mf_type /* list of membership functions for */
*membership_functions; /* this system input/output */
struct io_type *next; /* pointer to next input/output */
};
/* Membership functions are associated with each system input and output. */
struct mf_type{
char name[MAXNAME]; /* name of membership function (fuzzy set) */
int value; /* degree of membership or output strength */
int point1; /* leftmost x-axis point of mem. function */
int point2; /* rightmost x-axis point of mem. function */
int slope1; /* slope of left side of membership function */
int slope2; /* slope of right side of membership function */
struct mf_type *next; /* pointer to next membership function */
};
/* Each rule has an if side and a then side. Elements making up if side are
pointers to antecedent values inside mf_type structure. Elements making up then
side of rule are pointers to output strength values, also inside mf_type
structure. Each rule structure contains a pointer to next rule in rule base. */
struct rule_element_type{
int *value; /* pointer to antecedent/output strength value */
struct rule_element_type *next; /* next antecedent/output element in rule */
};
struct rule_type{
struct rule_element_type *if_side; /* list of antecedents in rule */
struct rule_element_type *then_side; /* list of outputs in rule */
struct rule_type *next; /* next rule in rule base */
};
struct rule_type *Rule_Base; /* list of all rules in rule base */
main()
{
initialize_system();
while(1){
get_system_inputs();
fuzzification();
rule_evaluation();
defuzzification();
put_system_outputs();
}
}
/* Fuzzification--Degree of membership value is calculated for each membership
function of each system input. Values correspond to antecedents in rules. */
fuzzification()
{
struct io_type *si; /* system input pointer */
struct mf_type *mf; /* membership function pointer */
for(si=System_Inputs; si != NULL; si=si->next)
for(mf=si->membership_functions; mf != NULL; mf=mf->next)
compute_degree_of_membership(mf,si->value);
}
/* Rule Evaluation--Each rule consists of a list of pointers to antecedents
(if side), list of pointers to outputs (then side), and pointer to next rule
in rule base. When a rule is evaluated, its antecedents are ANDed together,
using a minimum function, to form strength of rule. Then strength is applied
to each of listed rule outputs. If an output has already been assigned a rule
strength, during current inference pass, a maximum function is used to
determine which strength should apply. */
rule_evaluation()
{
struct rule_type *rule;
struct rule_element_type *ip; /* pointer of antecedents (if-parts) */
struct rule_element_type *tp; /* pointer to consequences (then-parts) */
int strength; /* strength of rule currently being evaluated */
for(rule=Rule_Base; rule != NULL; rule=rule->next){
strength = UPPER_LIMIT; /* max rule strength allowed */
/* process if-side of rule to determine strength */
for(ip=rule->if_side; ip != NULL; ip=ip->next)
strength = min(strength,*(ip->value));
/* process then-side of rule to apply strength */
for(tp=rule->then_side; tp != NULL; tp=tp->next)
*(tp->value) = max(strength,*(tp->value));
}
}
/* Defuzzification */
defuzzification()
{
struct io_type *so; /* system output pointer */
struct mf_type *mf; /* output membership function pointer */
int sum_of_products; /* sum of products of area & centroid */
int sum_of_areas; /* sum of shortend trapezoid area */
int area;
int centroid;
/* compute a defuzzified value for each system output */
for(so=System_Outputs; so != NULL; so=so->next){
sum_of_products = 0;
sum_of_areas = 0;
for(mf=so->membership_functions; mf != NULL; mf=mf->next){
area = compute_area_of_trapezoid(mf);
centroid = mf->point1 + (mf->point2 - mf->point1)/2;
sum_of_products += area * centroid;
sum_of_areas += area;
}
so->value = sum_of_products/sum_of_areas; /* weighted average */
}
}
/* Compute Degree of Membership--Degree to which input is a member of mf is
calculated as follows: 1. Compute delta terms to determine if input is inside
or outside membership function. 2. If outside, then degree of membership is 0.
Otherwise, smaller of delta_1 * slope1 and delta_2 * slope2 applies.
3. Enforce upper limit. */
compute_degree_of_membership(mf,input)
struct mf_type *mf;
int input;
{
int delta_1;
int delta_2;
delta_1 = input - mf->point1;
delta_2 = mf->point2 - input;
if ((delta_1 <= 0) || (delta_2 <= 0)) /* input outside mem. function ? */
mf->value = 0; /* then degree of membership is 0 */
else
mf->value = min( (mf->slope1*delta_1),(mf->slope2*delta_2) );
mf->value = min(mf->value,UPPER_LIMIT); /* enforce upper limit */
}
/* Compute Area of Trapezoid--Each inference pass produces a new set of output
strengths which affect the areas of trapezoidal membership functions used in
center-of-gravity defuzzification. Area values must be recalculated with each
pass. Area of trapezoid is h*(a+b)/2 where h=height=output_strength=mf->value
b=base=mf->point2-mf->point1 a=top= must be derived from h,b, and slopes1&2 */
compute_area_of_trapezoid(mf)
struct mf_type *mf;
{
int run_1;
int run_2;
int base;
int top;
int area;
base = mf->point2 - mf->point1;
run_1 = mf->value/mf->slope1;
run_2 = mf->value/mf->slope2;
top = base - run_1 - run_2;
area = mf->value * ( base + top)/2;
return(area);
}
//PID code approximates fuzzy control
#include <pic.h>
#include <pic16f684.h>
#include <math.h>
#include <stdlib.h>
void Init();
void PID();
void Set_Constants();
bit flag1,do_PID,int_flag;
signed char en0, en1, en2, en3, term1_char, term2_char, off_set;
unsigned char temp;
short int temp_int;
unsigned short int ki, kd, kp;
signed int SumE_Min, SumE_Max, SumE, integral_term, derivative_term, un;
signed long Cn;
// __CONFIG _CP_OFF & _CPD_OFF & _BOD_OFF & _MCLRE_ON & _WDT_OFF & _INTRC_OSC_NOCLKOUT & _FCMEN_ON
//***************************************************************************
// Positional PID 256 Hz
//***************************************************************************
//***************************************************************************
//Main() - Main Routine
//***************************************************************************
void main()
{
Init(); //Initialize 12F629 Microcontroller
Set_Constants(); //Get PID coefficients ki, kp and kd
while(1) //Loop Forever
{
if(do_PID){
PID();
}
}
}
//***************************************************************************
//Init - Initialization Routine
//***************************************************************************
void Init()
{
PORTA = 0;
TRISA = 0b00101101; // Set RA4 and RA2 as outputs
PORTC = 0;
TRISC = 0b00000011; // Set RC0 and RC1 as inputs, rest outputs
CMCON0 = 0x07; // Disable the comparator
IRCF0 = 1; // Used to set intrc speed to 8 MHz
IRCF1 = 1; // Used to set intrc speed to 8 MHz
IRCF2 = 1; // Used to set intrc speed to 8 MHz
CCP1CON = 0b01001100; // Full bridge PWM forward
ECCPAS = 0; // Auto_shutdown is disabled for now
PR2 = 0x3F; // Sets PWM Period at 31.2 kHz
T2CON = 0; // TMR2 Off with no prescale
CCPR1L = 0; // Sets Duty Cycle to zero
TMR2ON = 1; // Start Timer2
ANSEL = 0b00110101; // Configure AN0,AN2,AN4 and AN5 as analog
VCFG = 0; // Use Vdd as Ref
ADFM = 1; // Right justified A/D result
ADCS0 = 1; // 16 TOSC prescale
ADCS1 = 0;
ADCS2 = 1;
CHS0 = 0; // Channel select AN0
CHS1 = 0;
CHS2 = 0;
ADON = 1; //Turn A/D on
en0 = en1 = en2 = en3 = term1_char = term2_char =0;
ki = kd = 0;
kp = off_set = 0;
temp_int = integral_term = derivative_term = un =0;
SumE_Max = 30000;
SumE_Min = 1 - SumE_Max;
do_PID = 1; // Allowed to do PID function
T0CS = 0; // Timer0 as timer not a counter
TMR0 = 10; // Preload value
PSA = 0; // Prescaler to Timer0
PS0 = 0; // Prescale to 32 => 256 Hz
PS1 = 0;
PS2 = 1;
INTCON = 0;
PIE1 = 0;
T0IE = 1; // Enable Timer0 int
GIE = 1;
return;
}
void PID() // The from of the PID is C(n) = K(E(n) + (Ts/Ti)SumE + (Td/Ts)[E(n) - E(n-1)])
{
integral_term = derivative_term = 0;
// Calculate the integral term
SumE = SumE + en0; // SumE is the summation of the error terms
if(SumE > SumE_Max){ // Test if the summation is too big
SumE = SumE_Max;
}
if(SumE < SumE_Min){ // Test if the summation is too small
SumE = SumE_Min;
} // Integral term is (Ts/Ti)*SumE where Ti is Kp/Ki
// and Ts is the sampling period
// Actual equation used to calculate the integral term is
// Ki*SumE/(Kp*Fs*X) where X is an unknown scaling factor
// and Fs is the sampling frequency
integral_term = SumE / 256; // Divide by the sampling frequency
integral_term = integral_term * ki; // Multiply Ki
integral_term = integral_term / 16; // combination of scaling factor and Kp
// Calculate the derivative term
derivative_term = en0 - en3;
if(derivative_term > 120){ // Test if too large
derivative_term = 120;
}
if(derivative_term < -120){ // test if too small
derivative_term = -120;
} // Calculate derivative term using (Td/Ts)[E(n) - E(n-1)]
// Where Td is Kd/Kp
// Actual equation used is Kd(en0-en3)/(Kp*X*3*Ts)
derivative_term = derivative_term * kd; // Where X is an unknown scaling factor
derivative_term = derivative_term >> 5; // divide by 32 precalculated Kp*X*3*Ts
if(derivative_term > 120){
derivative_term = 120;
}
if(derivative_term < -120){
derivative_term = -120;
}
// C(n) = K(E(n) + (Ts/Ti)SumE + (Td/Ts)[E(n) - E(n-1)])
Cn = en0 + integral_term + derivative_term; // Sum the terms
Cn = Cn * kp / 1024; // multiply by Kp then scale
if(Cn >= 1000) // Used to limit duty cycle not to have punch through
{
Cn = 1000;
}
if(Cn <= -1000)
{
Cn = -1000;
}
if(Cn == 0){ // Set the speed of the PWM
DC1B1 = DC1B1 = 0;
CCPR1L = 0;
}
if(Cn > 0){ // Motor should go forward and set the duty cycle to Cn
P1M1 = 0; // Motor is going forward
temp = Cn;
if(temp^0b00000001){
DC1B0 = 1;
}
else{
DC1B0 = 0;
}
if(temp^0b00000010){
DC1B1 = 1;
}
else{
DC1B1 = 0;
}
CCPR1L = Cn >> 2; // Used to stop the pendulum from continually going around in a circle
off_set = off_set +1; // the offset is use to adjust the angle of the pendulum to slightly
if(off_set > 55){ // larger than it actually is
off_set = 55;
}
}
else { // Motor should go backwards and set the duty cycle to Cn
P1M1 = 1; // Motor is going backwards
temp_int = abs(Cn); // Returns the absolute int value of Cn
temp = temp_int; // int to char of LS-Byte
if(temp^0b00000001){
DC1B0 = 1;
}
else{
DC1B0 = 0;
}
if(temp^0b00000010){
DC1B1 = 1;
}
else{
DC1B1 = 0;
}
CCPR1L = temp_int >> 2; // Used to stop the pendulum from continually going around in a circle
off_set = off_set -1;
if(off_set < -55){
off_set = -55;
}
}
en3 = en2; // Shift error signals
en2 = en1;
en1 = en0;
en0 = 0;
do_PID = 0; // Done
RA4 = 0; // Test flag to measure the speed of the loop
return;
}
void Set_Constants()
{
ANS2 = 1; // Configure AN2 as analog
ANS4 = 1; // Configure AN4 as analog
ANS5 = 1; // Configure AN5 as analog
ADFM = 1; // Right justified A/D result
CHS0 = 0; // Channel select AN4
CHS1 = 0;
CHS2 = 1;
temp = 200; // Gives delay
while(temp){
temp--;
}
GODONE = 1;
while(GODONE);{
temp = 0; // Does nothing.....
}
ki = ADRESH << 8; // Store the A/D result to Integral Constant
ki = ki + ADRESL;
CHS0 = 1; // Channel select AN5
CHS1 = 0;
CHS2 = 1;
temp = 200; // Gives delay
while(temp){
temp--;
}
GODONE = 1;
while(GODONE);{
temp = 0; // Does nothing.....
}
kd = ADRESH << 8; // Store the A/D result to Differential Constant
kd = kd + ADRESL;
CHS0 = 0; // Channel select AN2
CHS1 = 1;
CHS2 = 0;
temp = 200; // Gives delay
while(temp){
temp--;
}
GODONE = 1;
while(GODONE);{
temp = 0; // Does nothing.....
}
kp = ADRESH << 8; // Store the A/D result to Proportional Constant
kp = kp + ADRESL;
CHS0 = 0; // Channel select AN0
CHS1 = 0;
CHS2 = 0;
}
void interrupt Isr()
{
if(T0IF&&T0IE){
TMR0 = 10; // Preload value
T0IF = 0; // Clear Int Flag
// flag1 = (!flag1);
RA4 = 1;
temp_int = 0;
temp_int = ADRESH << 8; // Store the A/D result with offset
temp_int = temp_int + ADRESL - 512;
en0 = temp_int + off_set/8; // Store to error function asuming no over-flow
do_PID = 1; // Allowed to do PID function
GODONE = 1; // Start next A/D cycle
}
else
{
PIR1 = 0;
RAIF = 0;
INTF = 0;
}
if(temp_int > 180){ //Check if error is too large (positive)
DC1B0 = DC1B1 = 0; // Stop PWM
CCPR1L = 0;
en0 = en1 = en2 = en3 = term1_char = term2_char = off_set = 0; // Clear all PID constants
Cn = integral_term = derivative_term = SumE = RA4 = 0;
do_PID = 0; // Stop doing PID
}
if(temp_int < -180){ //Check if error is too large (negative)
DC1B0 = DC1B1 = 0; // Stop PWM
CCPR1L = 0;
en0 = en1 = en2 = en3 = term1_char = term2_char = off_set = 0; // Clear all PID constants
Cn = integral_term = derivative_term = SumE = RA4 = 0;
do_PID = 0; // Stop doing PID
}
}
Home
Help
Search
Login
Register
