wsnell@sdcc13.ucsd.edu (Wesley Snell) (05/27/91)
#if 0
*****************************************************************************
Title: Truck Backer-Upper Demo
Description: Fuzzy Control System Demo Program
This program uses fuzzy control to backup a truck to a given
position and to arrive at the position (X = 200) at a given angle
(90 degrees). The truck may be moved by the user to establish an
initial truck position and angle. The program does not take into
account the Y coordinate of the truck position, therefore starting
positions for the truck demo should be made accordingly.
User commands are listed below and the function 'KeyCommand()' can
be examined for a more detailed explaination of their functions.
Commands: g, G Enable truck to GO
s, S STOP truck
l, L Turn steering wheel 1 degree to the LEFT
r, R Turn steering wheel 1 degree to the RIGHT
f, F Enable FUZZY Controller
t, T Toggle truck trail
c, C CLEAR demo area
n, N RESET truck demo
q, Q EXIT program
MODIFICATIONS
The fuzzy controller may be modified to improve its performance and
to test it. The fuzzy controller may be modified by changing its
rules and membership functions.
Rules may be modified by changing the consequent in 'gaiRules[]'.
Membership functions may be modified by changing the values used by
'gadbPosition[][]', 'gadbAngle[][]' and 'gadbWheelAngle[][]'.
compiler used: Microsoft C Optimizing Compiler Version 5.10
linker used: Microsoft Overlay Linker Version 3.65
programmer: Donald Carothers
3674 Ben St., San Diego CA 92111
date: 11/06/90
*****************************************************************************
#endif
#include <conio.h>
#include <stdio.h>
#include <graph.h>
#include <math.h>
#define loop while (1)
#define BOOL int
#define RadianToDegree(x) (x * (180.0 / 3.14159))
#define DegreeToRadian(x) (x * (3.14159 / 180.0))
#define min(x, y) ((x > y) ? y : x)
#define TRUE 1
#define FALSE 0
#define BLACK 0
#define BLUE 1
#define GREY 8
#define GREEN 10
#define CYAN 11
#define RED 12
#define YELLOW 14
#define WHITE 15
#define CLR_BCKGRND BLUE
#define CLR_TRUCK GREEN
#define CLR_TRUCK_TRAIL WHITE
#define CLR_ACTIVE_FG YELLOW
#define CLR_INACTIVE_FG GREY
/*
** Generic Fuzzy Indexes
*/
#define NL 0 /* Negative Large */
#define NM 1 /* Negative Medium */
#define NS 2 /* Negative Small */
#define ZE 3 /* Zero */
#define PS 4 /* Positive Small */
#define PM 5 /* Positive Medium */
#define PL 6 /* Positive Large */
/*
** Steering Wheel Output Indexes
*/
#define RL NL /* Right Large */
#define RM NM /* Right Medium */
#define RS NS /* Right Small */
#define CE ZE /* Centered */
#define LS PS /* Left Small */
#define LM PM /* Left Medium */
#define LL PL /* Left Large */
/*
** RULES
*/
/*
Each rule has two antecedents, and one consequent.
(truck position, truck angle, truck steering wheel angle)
*/
#define NUM_RULES 49
int gaiRules[NUM_RULES][3] =
{ NL, NL, LS,
NL, NM, LS,
NL, NS, RM,
NL, ZE, RM,
NL, PS, RL,
NL, PM, RL,
NL, PL, RL,
NM, NL, LL,
NM, NM, LM,
NM, NS, RS,
NM, ZE, RM,
NM, PS, RM,
NM, PM, RL,
NM, PL, RL,
NS, NL, LL,
NS, NM, LM,
NS, NS, LS,
NS, ZE, RS,
NS, PS, RM,
NS, PM, RM,
NS, PL, RL,
ZE, NL, LM,
ZE, NM, LM,
ZE, NS, LS,
ZE, ZE, CE,
ZE, PS, RS,
ZE, PM, RM,
ZE, PL, RM,
PS, NL, LL,
PS, NM, LM,
PS, NS, LM,
PS, ZE, LS,
PS, PS, RS,
PS, PM, RM,
PS, PL, RL,
PM, NL, LL,
PM, NM, LL,
PM, NS, LM,
PM, ZE, LM,
PM, PS, LS,
PM, PM, RM,
PM, PL, RL,
PL, NL, LL,
PL, NM, LL,
PL, NS, LL,
PL, ZE, LM,
PL, PS, LM,
PL, PM, RS,
PL, PL, RS
};
/*
** MEMBERSHIP FUNCTIONS
*/
/*
Each membership function is defined as a piecewise linear triangular
pulse with a maximum height of 1. Three values, in order of increasing
value, are used to specify each membership function:
(start of triangular pulse, pulse peak, end of triangular pulse)
*/
double gadbPosition[7][3] =
{ 0, 0, 100, /* NL Position = 0 */
50, 100, 150, /* NM Position = 100 */
120, 160, 200, /* NS Position = 160 */
180, 200, 220, /* ZE Position = 200 */
200, 240, 280, /* PS Position = 240 */
250, 300, 350, /* PM Position = 300 */
300, 400, 400 /* PL Position = 400 */
};
double gadbAngle[7][3] =
{ -100, -45, 10, /* NL Truck Angle = -45 */
-10, 20, 50, /* NM Truck Angle = 20 */
40, 65, 90, /* NS Truck Angle = 65 */
80, 90, 100, /* ZE Truck Angle = 90 */
90, 115, 140, /* PS Truck Angle = 115 */
130, 160, 190, /* PM Truck Angle = 160 */
170, 225, 280 /* PL Truck Angle = 225 */
};
double gadbWheelAngle[7][3] =
{ -40, -30, -15, /* NL Wheel Angle = -30 */
-25, -15, -5, /* NM Wheel Angle = -15 */
-12, -6, 0, /* NS Wheel Angle = -5 */
-5, 0, 5, /* ZE Wheel Angle = 0 */
0, 6, 12, /* PS Wheel Angle = 5 */
5, 15, 25, /* PM Wheel Angle = 15 */
15, 30, 40 /* PL Wheel Angle = 30 */
};
char *gaszInputIndx[7] = { "NL", "NM", "NS", "ZE", "PS", "PM", "PL" };
/*
** TRUCK Global Variables
*/
double gdbTruckX, gdbTruckY; /* Position of center of truck */
double gdbTruckAngle; /* Angle of truck with horizontal */
double gdbTruckWheelAngle; /* Angle of truck steering wheel */
BOOL gbTruckState; /* Truck moving (TRUE) */
BOOL gbFuzzyControl; /* Fuzzy controller on (TRUE) */
int giTruckTrailColor; /* Color of truck trail */
/*
** GLOBAL VARIABLES
*/
double gadbPositionMembership[7]; /* Storage for input results */
double gadbAngleMembership[7];
double gadbRuleResult[NUM_RULES]; /* Storage for rule outputs */
/*
** PROGRAM CODE
*/
main()
{ int x, y;
char c;
/* CONVERT degrees to radians */
for (x = NL ; x <= PL ; x++)
{ for (y = 0 ; y < 3 ; y++)
{ gadbAngle[x][y] = DegreeToRadian(gadbAngle[x][y]);
gadbWheelAngle[x][y] = DegreeToRadian(gadbWheelAngle[x][y]);
}
}
for (x = 0 ; x < NUM_RULES ; x++)
{ gadbRuleResult[x] = 0.0;
}
/* DEFAULT initial truck position */
gdbTruckX = 200.0; /* CENTER of truck */
gdbTruckY = 300.0;
gdbTruckAngle = DegreeToRadian(45.0); /* ANGLE of truck body with horizontal */
gdbTruckWheelAngle = 0.0; /* Truck steering wheel straight ahead */
gbTruckState = FALSE; /* Truck stopped */
gbFuzzyControl = FALSE; /* Fuzzy controller disabled */
giTruckTrailColor = CLR_BCKGRND;
_setvideomode((short) _VRES16COLOR); /* VGA 640 X 480 X 16 */
_setbkcolor(_BLUE); /* set background color */
_setcolor(WHITE);
_rectangle(_GBORDER, 0, 0, 400, 400); /* Demo area border */
_settextposition(18, 65);
_outtext("Position (X)");
_settextposition(19, 60);
_outtext("NL NM NS ZE PS PM PL");
x = 0;
for (y = 20 ; y < 27 ; y++)
{ _settextposition(y, 57);
_outtext(gaszInputIndx[x]);
x++;
}
DisplayTruckStatus(); /* update display of truck info */
_setcolor(CLR_TRUCK); /* set color to draw truck */
DrawTruck(); /* display truck at current position */
loop
{ if (kbhit()) /* is key waiting to be processed ? */
{ KeyCommand(); /* process key */
DisplayTruckStatus(); /* update display of truck info */
}
for (x = 0 ; x < 10000 ; x++) /* temporary time delay */
y = x * y;
if (gbTruckState) /* Check if truck moving */
{
/* Compute new output with fuzzy control system */
if (gbFuzzyControl) /* check if fuzzy control enabled */
ComputeOutput(); /* determine new output */
_setcolor(giTruckTrailColor); /* set color to erase truck */
DrawTruck(); /* ERASE old truck */
MoveTruck(); /* update truck info */
_setcolor(CLR_TRUCK); /* set color to draw truck */
DrawTruck(); /* display truck at new position */
DisplayTruckStatus(); /* update display of truck info */
}
} /* End of WHILE */
} /* End of 'main()' */
ComputeOutput()
{ int i;
double db, dbWeights, dbOut;
/* COMPUTE input membership */
for (i = NL ; i <= PL ; i++)
{ /* Position Membership */
if (((double) gdbTruckX >= gadbPosition[i][0]) && ((double) gdbTruckX <= gadbPosition[i][1]))
gadbPositionMembership[i] = ((double) gdbTruckX - gadbPosition[i][0]) * (1.0 / (gadbPosition[i][1] - gadbPosition[i][0]));
else if (((double) gdbTruckX >= gadbPosition[i][1]) && ((double) gdbTruckX <= gadbPosition[i][2]))
gadbPositionMembership[i] = (gadbPosition[i][2] - (double) gdbTruckX) * (1.0 / (gadbPosition[i][2] - gadbPosition[i][1]));
else
gadbPositionMembership[i] = 0.0;
/* Truck Angle Membership */
if ((gdbTruckAngle >= gadbAngle[i][0]) && (gdbTruckAngle <= gadbAngle[i][1]))
gadbAngleMembership[i] = (gdbTruckAngle - gadbAngle[i][0]) * (1.0 / (gadbAngle[i][1] - gadbAngle[i][0]));
else if ((gdbTruckAngle > gadbAngle[i][1]) && (gdbTruckAngle <= gadbAngle[i][2]))
gadbAngleMembership[i] = (gadbAngle[i][2] - gdbTruckAngle) * (1.0 / (gadbAngle[i][2] - gadbAngle[i][1]));
else
gadbAngleMembership[i] = 0.0;
}
/* Rules */
for (i = 0 ; i < NUM_RULES ; i++)
{ gadbRuleResult[i] = min(gadbPositionMembership[gaiRules[i][0]], gadbAngleMembership[gaiRules[i][1]]);
}
/* Compute output */
dbOut = 0.0;
dbWeights = 0.0;
for (i = 0 ; i < NUM_RULES ; i++)
{ if (gadbRuleResult[i] == 0.0)
continue; /* skip computation for non-firing rule */
/* Weight Value (trapezoidal area) */
db = (1.0 - gadbRuleResult[i]);
db = 0.5 * (gadbWheelAngle[gaiRules[i][2]][2] - gadbWheelAngle[gaiRules[i][2]][0]) * (1 - (db * db));
dbWeights += db;
dbOut += db * gadbWheelAngle[gaiRules[i][2]][1];
} /* End of FOR */
if (dbWeights != 0.0)
dbOut /= dbWeights;
gdbTruckWheelAngle = dbOut;
/* Impose Wheel limits */
if (gdbTruckWheelAngle > DegreeToRadian(30.0))
gdbTruckWheelAngle = DegreeToRadian(30.0);
else if (gdbTruckWheelAngle < DegreeToRadian(-30.0))
gdbTruckWheelAngle = DegreeToRadian(-30.0);
}
DrawTruck()
{ int i;
double dbAngle;
static struct xycoord sastVertices[7];
dbAngle = DegreeToRadian(45.0); /* 45 degrees */
for (i = 0 ; i < 5 ; i++)
{ sastVertices[i].xcoord = (int) (gdbTruckX + (4 * 5 * cos(gdbTruckAngle + dbAngle)));
sastVertices[i].ycoord = (int) (gdbTruckY - (4 * 5 * sin(gdbTruckAngle + dbAngle)));
dbAngle += DegreeToRadian(90.0); /* next truck corner */
}
sastVertices[5].xcoord = (int) (gdbTruckX + (4 * 4 * cos(gdbTruckAngle - DegreeToRadian(110.0))));
sastVertices[5].ycoord = (int) (gdbTruckY - (4 * 4 * sin(gdbTruckAngle - DegreeToRadian(110.0))));
sastVertices[6].xcoord = (int) (gdbTruckX + (4 * 4 * cos(gdbTruckAngle - DegreeToRadian(250.0))));
sastVertices[6].ycoord = (int) (gdbTruckY - (4 * 4 * sin(gdbTruckAngle - DegreeToRadian(250.0))));
_moveto(sastVertices[0].xcoord, sastVertices[0].ycoord);
for (i = 1 ; i < 5 ; i++)
_lineto(sastVertices[i].xcoord, sastVertices[i].ycoord);
_moveto(sastVertices[5].xcoord, sastVertices[5].ycoord);
_lineto(sastVertices[6].xcoord, sastVertices[6].ycoord);
} /* End of 'DrawTruck()' */
DisplayTruckStatus()
{ int i, x, y;
static char sszOut[40];
static char *saszInputIndx[7] = { "NL", "NM", "NS", "ZE", "PS", "PM", "PL" };
static char *saszOutputIndx[7] = { "RL", "RM", "RS", "CE", "LS", "LM", "LL" };
_settextcolor(WHITE);
_setbkcolor(_BLUE);
/* Display Truck Position */
_settextposition(3, 53);
sprintf(sszOut, "Position: X=%5.1lf, Y=%5.1lf", gdbTruckX, gdbTruckY);
_outtext(sszOut);
/* Display Truck Angle */
_settextposition(4, 53);
sprintf(sszOut, "Truck Angle: %6.1lfx", RadianToDegree(gdbTruckAngle));
_outtext(sszOut);
/* Display Truck Steering Wheel Angle */
_settextposition(6, 53);
sprintf(sszOut, "Wheel Angle: %6.1lfx", RadianToDegree(gdbTruckWheelAngle));
_outtext(sszOut);
/* Display Rule status */
i = 0;
for (x = 60 ; x < 81 ; x += 3)
{ for (y = 20 ; y < 27 ; y += 1)
{ if (gadbRuleResult[i] == 0.0)
_settextcolor(CLR_INACTIVE_FG);
else
_settextcolor(CLR_ACTIVE_FG);
_settextposition(y, x);
_outtext(saszOutputIndx[gaiRules[i][2]]);
i++; /* next rule */
}
}
_setbkcolor(_BLUE);
_settextcolor(WHITE);
}
MoveTruck()
{
gdbTruckX += (4 * cos(gdbTruckAngle));
gdbTruckY -= (4 * sin(gdbTruckAngle));
if ((gdbTruckY > 380.0) || (gdbTruckX > 380.0) || (gdbTruckX < 20.0))
{ gdbTruckX -= (4 * cos(gdbTruckAngle));
gdbTruckY += (4 * sin(gdbTruckAngle));
}
else if (gdbTruckY < 20.0) /* check if simulation complete */
{ gbTruckState = FALSE; /* stop truck at boundary */
}
else
{ gdbTruckAngle += gdbTruckWheelAngle;
while (gdbTruckAngle > DegreeToRadian(270.0))
gdbTruckAngle -= DegreeToRadian(360.0);
while (gdbTruckAngle < DegreeToRadian(-90.0))
gdbTruckAngle += DegreeToRadian(360.0);
}
}
KeyCommand()
{ int c, x, y;
c = getch();
switch(c)
{ case (int) 'G': /* GO command */
case (int) 'g':
gbTruckState = TRUE;
break;
case (int) 'S': /* STOP command */
case (int) 's':
gbTruckState = FALSE;
break;
case (int) 'L': /* LEFT (increase wheel angle 1 degree) */
case (int) 'l':
gdbTruckWheelAngle += DegreeToRadian(1.0);
break;
case (int) 'R': /* RIGHT (decrease wheel angle 1 degree) */
case (int) 'r':
gdbTruckWheelAngle -= DegreeToRadian(1.0);
break;
case (int) 'F': /* Toggle Fuzzy Control */
case (int) 'f':
if (gbFuzzyControl)
gbFuzzyControl = FALSE;
else
gbFuzzyControl = TRUE;
break;
case (int) 'T': /* Toggle truck trail */
case (int) 't':
if (giTruckTrailColor == CLR_BCKGRND)
giTruckTrailColor = CLR_TRUCK_TRAIL;
else
giTruckTrailColor = CLR_BCKGRND;
break;
case (int) 'N': /* Reset simulation */
case (int) 'n':
_setcolor(CLR_BCKGRND); /* set color to erase truck */
DrawTruck(); /* ERASE old truck */
gdbTruckX = 200.0; /* CENTER of truck */
gdbTruckY = 300.0;
gdbTruckAngle = DegreeToRadian(45.0); /* ANGLE of truck body with vertical */
gdbTruckWheelAngle = 0.0;
gbTruckState = FALSE;
gbFuzzyControl = FALSE;
DisplayTruckStatus();
_setcolor(CLR_TRUCK); /* set color to draw truck */
DrawTruck();
break;
case (int) 'C': /* CLEAR demo area command */
case (int) 'c':
_clearscreen(_GCLEARSCREEN);
_setcolor(WHITE);
_rectangle(_GBORDER, 0, 0, 400, 400); /* Demo area border */
_settextposition(18, 65);
_outtext("Position (X)");
_settextposition(19, 60);
_outtext("NL NM NS ZE PS PM PL");
x = 0;
for (y = 20 ; y < 27 ; y++)
{ _settextposition(y, 57);
_outtext(gaszInputIndx[x]);
x++;
}
DisplayTruckStatus(); /* update display of truck info */
_setcolor(CLR_TRUCK); /* set color to draw truck */
DrawTruck(); /* display truck at current position */
break;
case (int) 'Q': /* QUIT program command */
case (int) 'q':
_setvideomode((short) _DEFAULTMODE); /* TEXT 80 X 25 X 16 */
exit(0);
default:
break;
} /* End of SWITCH */
/* TRUCK steering wheel limits */
if (gdbTruckWheelAngle > DegreeToRadian(30.0))
gdbTruckWheelAngle = DegreeToRadian(30.0);
else if (gdbTruckWheelAngle < DegreeToRadian(-30.0))
gdbTruckWheelAngle = DegreeToRadian(-30.0);
} /* End of 'KeyCommand()' */
/*----------------------------- END OF FILE -----------------------------*/pekka@latcs1.lat.oz.au (Pekka Isomursu) (05/31/91)
In article <19792@sdcc6.ucsd.edu> wsnell@sdcc13.ucsd.edu (Wesley Snell) gives
us C source code for a Fuzzy Control System Demo Program.
Would someone please point me out other sources of public domain fuzzy
logic source code? Language doesn't matter, although C is preferable.
Thanks in advance.
Pekka Isomursu
Technical Research Centre of Finland, Oulu, Finland
visiting La Trobe University, Melbourne, Australia