//GRUPPE Ilia- David && Andreas Wolf
//Datum 31.12.2005 23:59:59
//Blatt 4 Aufgabe 4.1 (a,b,c,d,!e)
//Aufgabe 4.1(b)
//
// Hier_drüber_schwingen = -(L1*cos(theta_1)+(L2*cos(theta_1+theta2)))
// wobei L1 = L2 = 1
#include "acrobot.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#define NVARS 4
#define NREGELN 6
#define IRRELEVANT -1
#define NSCHRITTE 100
#define NKNOTEN 3
#define RECHTS 2
#define MITTE 1
#define LINKS 0
// ===================================================================
// V A R I A B L E N
// ===================================================================
typedef struct variable{
int nknoten;
double start, ende ,delta;
} variable;
variable varmenge[NVARS];
variable var_u;
// ===================================================================
// R E G E L N
// ===================================================================
typedef struct regel{
int praemisse[NVARS];
int konklusion;
} regel;
regel regelmenge[NREGELN];
// ===================================================================
// F U N K T I O N E N
// ===================================================================
//Berechnet die Zugehoerigkeitsfunktion (berechne_erfuellungsgrad_pro_variable)
double zugehoerigkeitsfunktion(int regel, int variable, double wert){
int varindex;
varindex=regelmenge[regel].praemisse[variable];
if(varindex==IRRELEVANT) {return 0;}
double delta;
delta=varmenge[variable].delta;
double xi;
xi=varmenge[variable].start+delta*varindex;
//Abbruchbedingung
if(varindex<0 || varindex>varmenge[variable].nknoten){
printf("\n Fehler in Funktion \"berechne_erfuellungsgrad_pro_variable\"!\n");
return 0;
}
//links:
if((wert>=(xi-delta)) && (wert<=xi)){
return ((wert-(xi-delta))/delta);
}
//rechts:
if((wert>xi) && (wert<=(xi+delta))){
return ((xi+delta-wert)/delta);
}
return 0;
}
//Liefert das Minimum (UND-Verknüpfung) aller Zugehörigkeitsfunktionen
double erfuellung_pro_regel(int regel, double *zustand){
int i;
double min=1, temp;
for(i=0; i<NVARS; i++){
if(regelmenge[regel].praemisse[i] !=IRRELEVANT){
temp=zugehoerigkeitsfunktion(regel, i, zustand[i]);
if(temp<min){
min=temp;
}
}
}
return min;
}
double fuzzy_steuerung(double *zustand){
int i;
double flaeche, zaehler=0, nenner=0, xi;
//Formel s. Aufgabenblatt
for(i=0; i<NREGELN; i++){
flaeche=erfuellung_pro_regel(i, zustand);
xi=var_u.start+var_u.delta*regelmenge[i].konklusion;
zaehler+=flaeche*xi;
nenner+=flaeche;
}
if(nenner==0){
return 1;
}
return zaehler/nenner;
}
// **********************************************************************
// * M A I N *
// **********************************************************************
int main(){
int i, j, z;
double s[NVARS], ss[NVARS], u;
double t=0;
FILE *stream;
//Initialisierung:
for(i=0; i<NVARS; i++){
s[i]=0;
}
// ===================================================================
// D e f i n i t i o n d e r V a r i a b l e n
// ===================================================================
//Ausgangsvariable u:
var_u.start=-1;
var_u.ende=1;
var_u.nknoten=3; //u hat die Mengen "nach hinten" (-1), "mitte" (0) und "nach vorne" (1);
var_u.delta=1;
//Die Grenzen entsprechen den Vorgaben des Aufgabenblattes
varmenge[0].start=varmenge[1].start=-M_PI;
varmenge[0].ende=varmenge[1].ende=M_PI;
varmenge[2].start=-4*M_PI;
varmenge[2].ende=4*M_PI;
varmenge[3].start=-9*M_PI;
varmenge[3].ende=9*M_PI;
varmenge[0].delta=varmenge[1].delta=M_PI;
varmenge[2].delta=4*M_PI;
varmenge[3].delta=9*M_PI;
for(i=0; i<NVARS; i++){
varmenge[i].nknoten=NKNOTEN; //alle Variablen in 3 Mengen unterteilt: N, ZE, P
// varmenge[i].delta=((-(varmenge[i].start)+varmenge[i].ende)/(varmenge[i].nknoten-1)); //delta errechnet sich für alle gleich
}
// ===================================================================
// D e f i n i t i o n d e r R e g e l n
// ===================================================================
//TESTREGELN für NKNOTEN = 3*****************************************
//WENN theta_1 = LINKS UND theta_1_punkt = LINKS DANN KRAFT nach RECHTS...
//Oberkörper ist links und er schwingt nach links
regelmenge[0].praemisse[0]=LINKS;
regelmenge[0].praemisse[1]=IRRELEVANT;
regelmenge[0].praemisse[2]=LINKS;
regelmenge[0].praemisse[3]=IRRELEVANT;
regelmenge[0].konklusion=RECHTS;
//Oberkörper ist rechts und schwingt nach rechts
regelmenge[1].praemisse[0]=RECHTS;
regelmenge[1].praemisse[1]=IRRELEVANT;
regelmenge[1].praemisse[2]=RECHTS;
regelmenge[1].praemisse[3]=IRRELEVANT;
regelmenge[1].konklusion=LINKS;
//Oberkörper hängt mittig und schwingt nach rechts
regelmenge[2].praemisse[0]=MITTE;
regelmenge[2].praemisse[1]=IRRELEVANT;
regelmenge[2].praemisse[2]=RECHTS;
regelmenge[2].praemisse[3]=IRRELEVANT;
regelmenge[2].konklusion=LINKS;
//Oberkörper hängt mittig und schwingt nach links
regelmenge[3].praemisse[0]=MITTE;
regelmenge[3].praemisse[1]=IRRELEVANT;
regelmenge[3].praemisse[2]=LINKS;
regelmenge[3].praemisse[3]=IRRELEVANT;
regelmenge[3].konklusion=RECHTS;
//Oberkörper mittig, Füße links und schwingt nicht
regelmenge[4].praemisse[0]=MITTE;
regelmenge[4].praemisse[1]=LINKS;
regelmenge[4].praemisse[2]=IRRELEVANT;
regelmenge[4].praemisse[3]=LINKS;
regelmenge[4].konklusion=LINKS;
//Oberkörper mittig, Füße rechts und schwingt nicht
regelmenge[5].praemisse[0]=MITTE;
regelmenge[5].praemisse[1]=RECHTS;
regelmenge[5].praemisse[2]=IRRELEVANT;
regelmenge[5].praemisse[3]=RECHTS;
regelmenge[5].konklusion=RECHTS;
//n Schritte simulieren, und testen, ob Ziel erreicht
stream=fopen("egal.txt","w+");
z=0;
for(i=0; i<NSCHRITTE; i++){
//Abbruchbedingung:
double a ;
a = -(cos(s[0])+(cos(s[0]+s[2])));
if(a >= 1) {
printf("Das Ziel wurde in %d erreicht.\n", z);
system("PAUSE");
return 0;
}
z++;
u=fuzzy_steuerung(s);
step(s, u, ss);
printf("s=%.2f %.2f %.2f %.2f >> u=%.2f >> ss=%.2f %.2f %.2f %.2f \n", s[0], s[1], s[2], s[3], u, ss[0], ss[1], ss[2], ss[3]);
t += 0.2;
fprintf(stream, "%.2f %.2f %.2f %.2f %.2f %.1f\n", s[0], s[1], s[2], s[3], u, t);
// Neuer Zustand --> Alter Zustand
for (j=0;j<NVARS;j++) s[j]=ss[j];
}
fclose(stream);
printf("Das Ziel wurde nach %d Schritten nicht erreicht.\n", NSCHRITTE);
system("PAUSE");
return 0;
}