home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
AMIGA PD 1
/
AMIGA-PD-1.iso
/
Meeting_Pearls_II
/
html
/
sw
/
nbsd
/
FuzzyPendel
/
balance.c
< prev
next >
Wrap
C/C++ Source or Header
|
1994-06-28
|
4KB
|
157 lines
#include <curses.h>
#include <math.h>
#include "balance.h"
#define pi M_PI
#define NB 0
#define NM 1
#define NS 2
#define ZE 3
#define PS 4
#define PM 5
#define PB 6
#define NRULES 7
static double b_alpha_l[NRULES],b_alpha_u[NRULES];
static double b_alphaDot_l[NRULES],b_alphaDot_u[NRULES];
static int b_rule_alpha[NRULES];
static int b_rule_alphaDot[NRULES];
static int b_rule_a[NRULES];
static char *b_bez[NRULES];
/*************************************************************************/
void balance_init(void)
{
int i;
/* Grenzen fuer Alpha */
b_alpha_l[NB] = -30.0; b_alpha_u[NB] = 30.0;
b_alpha_l[NM] = 0.0; b_alpha_u[NM] = 60.0;
b_alpha_l[NS] = 30.0; b_alpha_u[NS] = 90.0;
b_alpha_l[ZE] = 60.0; b_alpha_u[ZE] = 120.0;
b_alpha_l[PS] = 90.0; b_alpha_u[PS] = 150.0;
b_alpha_l[PM] = 120.0; b_alpha_u[PM] = 180.0;
b_alpha_l[PB] = 150.0; b_alpha_u[PB] = 210.0;
for(i=0;i<NRULES;i++){
b_alpha_l[i] = b_alpha_l[i]*pi/180.0;
b_alpha_u[i] = b_alpha_u[i]*pi/180.0;
}
/* Grenzen fuer alphaDot */
b_alphaDot_l[NB] = -5.0; b_alphaDot_u[NB] = -1.0;
b_alphaDot_l[NM] = -3.0; b_alphaDot_u[NM] = -1.0;
b_alphaDot_l[NS] = -2.0; b_alphaDot_u[NS] = 1.0;
b_alphaDot_l[ZE] = -0.5; b_alphaDot_u[ZE] = 0.5;
b_alphaDot_l[PS] = -1.0; b_alphaDot_u[PS] = 2.0;
b_alphaDot_l[PM] = 1.0; b_alphaDot_u[PM] = 3.0;
b_alphaDot_l[PB] = 1.0; b_alphaDot_u[PB] = 5.0;
/* Regeln definieren */
b_rule_alpha[0]=PM; b_rule_alphaDot[0]=ZE; b_rule_a[0]=PM;
b_rule_alpha[1]=PS; b_rule_alphaDot[1]=PS; b_rule_a[1]=PS;
b_rule_alpha[2]=PS; b_rule_alphaDot[2]=NS; b_rule_a[2]=ZE;
b_rule_alpha[3]=NM; b_rule_alphaDot[3]=ZE; b_rule_a[3]=NM;
b_rule_alpha[4]=NS; b_rule_alphaDot[4]=NS; b_rule_a[4]=NS;
b_rule_alpha[5]=NS; b_rule_alphaDot[5]=PS; b_rule_a[5]=ZE;
b_rule_alpha[6]=ZE; b_rule_alphaDot[6]=ZE; b_rule_a[6]=ZE;
/* Bezeichnungen fuer Indizes */
b_bez[NB] = "NB";
b_bez[NM] = "NM";
b_bez[NS] = "NS";
b_bez[ZE] = "ZE";
b_bez[PS] = "PS";
b_bez[PM] = "PM";
b_bez[PB] = "PB";
}
/*************************************************************************/
double balance(double alpha, double alphaDot)
{
double b_alpha[NRULES];
double b_alphaDot[NRULES];
double b_Fa[NRULES];
double max,mid;
int i;
/* Gewichte von alpha ausrechnen */
for(i=0;i<NRULES;i++){
if((b_alpha_u[i] > alpha) && (alpha > b_alpha_l[i])){
mid = (b_alpha_u[i]+b_alpha_l[i])/2.0;
if(alpha>mid){
/* linke Haelfte */
b_alpha[i] = 1.0-(alpha-mid)/(b_alpha_u[i]-mid);
}else{
/* rechte Haelfte */
b_alpha[i] = (alpha-b_alpha_l[i])/(mid-b_alpha_l[i]);
}
}else{
b_alpha[i] = 0.0;
}
}
/* Gewichte von alphaDot ausrechnen */
for(i=0;i<NRULES;i++){
if((b_alphaDot_u[i] > alphaDot) && (alphaDot > b_alphaDot_l[i])){
mid = (b_alphaDot_u[i]+b_alphaDot_l[i])/2.0;
if(alphaDot>mid){
/* rechte Haelfte */
b_alphaDot[i] = 1.0-(alphaDot-mid)/(b_alphaDot_u[i]-mid);
}else{
/* linke Haelfte */
b_alphaDot[i] = (alphaDot-b_alphaDot_l[i])/(mid-b_alphaDot_l[i]);
}
}else{
b_alphaDot[i] = 0.0;
}
}
/* Regeln anwenden */
for(i=0;i<NRULES;i++){
b_Fa[i] = 0.0;
}
for(i=0;i<NRULES;i++){
b_Fa[b_rule_a[i]] = Max(b_Fa[b_rule_a[i]],
Min(b_alpha[b_rule_alpha[i]],
b_alphaDot[b_rule_alphaDot[i]]));
}
/* Mittelwert ausrechnen */
max=0.0;
for(i=0;i<NRULES;i++){
max =max + (i-(NRULES/2))*b_Fa[i];
}
/* alpha ausgeben */
gotoxy(58,3); printw("alpha:");
for(i=0;i<NRULES;i++){
gotoxy(58,4+i);
printw("%s: %5.3f",b_bez[i],b_alpha[i]);
};
/* alphaDot ausgeben */
gotoxy(70,3); printw("alphaDot:");
for(i=0;i<NRULES;i++){
gotoxy(70,4+i);
printw("%s: %5.3f",b_bez[i],b_alphaDot[i]);
};
/* b_Fa ausgeben */
gotoxy(65,13); printw("b_Fa:");
for(i=0;i<NRULES;i++){
gotoxy(65,14+i);
printw("%s: %5.3f",b_bez[i],b_Fa[i]);
};
/* max ausgeben */
gotoxy(65,22);
printw("max=%6.3f",max);
refresh();
return -max;
}