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 >
C/C++ Source or Header  |  1994-06-28  |  4KB  |  157 lines
  1. #include <curses.h>
  2. #include <math.h>
  3. #include "balance.h"
  4.  
  5. #define pi M_PI
  6.  
  7. #define NB               0
  8. #define NM               1
  9. #define NS               2
  10. #define ZE               3
  11. #define PS               4 
  12. #define PM               5
  13. #define PB               6
  14.  
  15. #define NRULES 7
  16.  
  17. static double b_alpha_l[NRULES],b_alpha_u[NRULES];
  18. static double b_alphaDot_l[NRULES],b_alphaDot_u[NRULES];
  19. static int    b_rule_alpha[NRULES];
  20. static int    b_rule_alphaDot[NRULES];
  21. static int    b_rule_a[NRULES];
  22. static char  *b_bez[NRULES];
  23.  
  24. /*************************************************************************/
  25. void balance_init(void)
  26. {
  27.     int i;
  28.  
  29.     /* Grenzen fuer Alpha */
  30.     b_alpha_l[NB] = -30.0;   b_alpha_u[NB] =  30.0;
  31.     b_alpha_l[NM] =   0.0;   b_alpha_u[NM] =  60.0;
  32.     b_alpha_l[NS] =  30.0;   b_alpha_u[NS] =  90.0;
  33.     b_alpha_l[ZE] =  60.0;   b_alpha_u[ZE] = 120.0;
  34.     b_alpha_l[PS] =  90.0;   b_alpha_u[PS] = 150.0;
  35.     b_alpha_l[PM] = 120.0;   b_alpha_u[PM] = 180.0;
  36.     b_alpha_l[PB] = 150.0;   b_alpha_u[PB] = 210.0;
  37.  
  38.     for(i=0;i<NRULES;i++){
  39.     b_alpha_l[i] = b_alpha_l[i]*pi/180.0;
  40.     b_alpha_u[i] = b_alpha_u[i]*pi/180.0;
  41.     }
  42.  
  43.     /* Grenzen fuer alphaDot */
  44.     b_alphaDot_l[NB] = -5.0;   b_alphaDot_u[NB] = -1.0;
  45.     b_alphaDot_l[NM] = -3.0;   b_alphaDot_u[NM] = -1.0;
  46.     b_alphaDot_l[NS] = -2.0;   b_alphaDot_u[NS] =  1.0;
  47.     b_alphaDot_l[ZE] = -0.5;   b_alphaDot_u[ZE] =  0.5;
  48.     b_alphaDot_l[PS] = -1.0;   b_alphaDot_u[PS] =  2.0;
  49.     b_alphaDot_l[PM] =  1.0;   b_alphaDot_u[PM] =  3.0;
  50.     b_alphaDot_l[PB] =  1.0;   b_alphaDot_u[PB] =  5.0;
  51.  
  52.     /* Regeln definieren */
  53.     b_rule_alpha[0]=PM; b_rule_alphaDot[0]=ZE; b_rule_a[0]=PM;
  54.     b_rule_alpha[1]=PS; b_rule_alphaDot[1]=PS; b_rule_a[1]=PS;
  55.     b_rule_alpha[2]=PS; b_rule_alphaDot[2]=NS; b_rule_a[2]=ZE;
  56.     b_rule_alpha[3]=NM; b_rule_alphaDot[3]=ZE; b_rule_a[3]=NM;
  57.     b_rule_alpha[4]=NS; b_rule_alphaDot[4]=NS; b_rule_a[4]=NS;
  58.     b_rule_alpha[5]=NS; b_rule_alphaDot[5]=PS; b_rule_a[5]=ZE;
  59.     b_rule_alpha[6]=ZE; b_rule_alphaDot[6]=ZE; b_rule_a[6]=ZE;
  60.  
  61.     /* Bezeichnungen fuer Indizes */
  62.     b_bez[NB] = "NB";
  63.     b_bez[NM] = "NM";
  64.     b_bez[NS] = "NS";
  65.     b_bez[ZE] = "ZE";
  66.     b_bez[PS] = "PS";
  67.     b_bez[PM] = "PM";
  68.     b_bez[PB] = "PB";
  69. }
  70.  
  71. /*************************************************************************/
  72. double balance(double alpha, double alphaDot)
  73. {
  74.     double b_alpha[NRULES];
  75.     double b_alphaDot[NRULES];
  76.     double b_Fa[NRULES];
  77.     double max,mid;
  78.     int i;
  79.  
  80.     /* Gewichte von alpha ausrechnen */
  81.     for(i=0;i<NRULES;i++){
  82.     if((b_alpha_u[i] > alpha) && (alpha > b_alpha_l[i])){
  83.         mid = (b_alpha_u[i]+b_alpha_l[i])/2.0;
  84.         if(alpha>mid){
  85.         /* linke Haelfte */
  86.         b_alpha[i] = 1.0-(alpha-mid)/(b_alpha_u[i]-mid);
  87.         }else{
  88.         /* rechte Haelfte */
  89.         b_alpha[i] = (alpha-b_alpha_l[i])/(mid-b_alpha_l[i]);
  90.         }
  91.     }else{
  92.         b_alpha[i] = 0.0;
  93.     }
  94.     }
  95.  
  96.     /* Gewichte von alphaDot ausrechnen */
  97.     for(i=0;i<NRULES;i++){
  98.     if((b_alphaDot_u[i] > alphaDot) && (alphaDot > b_alphaDot_l[i])){
  99.         mid = (b_alphaDot_u[i]+b_alphaDot_l[i])/2.0;
  100.         if(alphaDot>mid){
  101.         /* rechte Haelfte */
  102.         b_alphaDot[i] = 1.0-(alphaDot-mid)/(b_alphaDot_u[i]-mid);
  103.         }else{
  104.         /* linke Haelfte */
  105.         b_alphaDot[i] = (alphaDot-b_alphaDot_l[i])/(mid-b_alphaDot_l[i]);
  106.         }
  107.     }else{
  108.         b_alphaDot[i] = 0.0;
  109.     }
  110.     }
  111.  
  112.     /* Regeln anwenden */
  113.     for(i=0;i<NRULES;i++){
  114.     b_Fa[i] = 0.0;
  115.     }
  116.     for(i=0;i<NRULES;i++){
  117.     b_Fa[b_rule_a[i]] = Max(b_Fa[b_rule_a[i]],
  118.                 Min(b_alpha[b_rule_alpha[i]],
  119.                     b_alphaDot[b_rule_alphaDot[i]]));
  120.     }
  121.  
  122.     /* Mittelwert ausrechnen */
  123.     max=0.0;
  124.     for(i=0;i<NRULES;i++){
  125.     max =max + (i-(NRULES/2))*b_Fa[i];
  126.     }
  127.  
  128.     /* alpha ausgeben */
  129.     gotoxy(58,3); printw("alpha:");
  130.     for(i=0;i<NRULES;i++){
  131.     gotoxy(58,4+i);
  132.     printw("%s: %5.3f",b_bez[i],b_alpha[i]);
  133.     };
  134.     
  135.     /* alphaDot ausgeben */
  136.     gotoxy(70,3); printw("alphaDot:");
  137.     for(i=0;i<NRULES;i++){
  138.     gotoxy(70,4+i);
  139.     printw("%s: %5.3f",b_bez[i],b_alphaDot[i]);
  140.     };
  141.     
  142.     /* b_Fa ausgeben */
  143.     gotoxy(65,13); printw("b_Fa:");
  144.     for(i=0;i<NRULES;i++){
  145.     gotoxy(65,14+i);
  146.     printw("%s: %5.3f",b_bez[i],b_Fa[i]);
  147.     };
  148.     
  149.     /* max ausgeben */
  150.     gotoxy(65,22);
  151.     printw("max=%6.3f",max);
  152.  
  153.     refresh();
  154.  
  155.     return -max;
  156. }
  157.