home *** CD-ROM | disk | FTP | other *** search
/ AMIGA PD 1 / AMIGA-PD-1.iso / Meeting_Pearls_II / html / sw / nbsd / FuzzyPendel / pendel.pas < prev    next >
Pascal/Delphi Source File  |  1994-06-28  |  8KB  |  293 lines
  1. program pendel;
  2. uses crt;
  3.  
  4. const
  5.      pi        = 3.141592654;
  6.      g         = 9.81;
  7.  
  8.      m         = 1.0;
  9.      l         = 1.0;
  10.      deltaT    = 0.025;
  11.      alpha0    = 60*(pi/180.0);
  12.      alphaDot0 = 1.0;
  13.      a0        = 500.0;
  14.      a0_cheat  = 0.0;
  15.  
  16.      Bumper_alphaDot = 0.5;
  17.      Bumper_alpha    = 10.0*(pi/180.0);
  18.  
  19.      Xcenter   = 30;
  20.      Ycenter   = 23;
  21.      Xscale    = 25.0;
  22.      Yscale    = 20.0;
  23.  
  24.      NB        = 0;
  25.      NM        = 1;
  26.      NS        = 2;
  27.      ZE        = 3;
  28.      PS        = 4;
  29.      PM        = 5;
  30.      PB        = 6;
  31.  
  32. var
  33.    alpha,
  34.    Fa,
  35.    Fres,
  36.    Fz,
  37.    alphaDot,
  38.    deltaAlpha,
  39.    gamma,
  40.    Fg,
  41.    beta:          real;
  42.    n:             integer;
  43.    c:             char;
  44.    kx,ky:         integer;
  45.  
  46. var
  47.    b_alpha_l: array[0..6] of real;
  48.    b_alpha_u: array[0..6] of real;
  49.    b_alphaDot_l: array[0..6] of real;
  50.    b_alphaDot_u: array[0..6] of real;
  51.    b_rule_alpha: array[0..6] of integer;
  52.    b_rule_alphaDot: array[0..6] of integer;
  53.    b_rule_a: array[0..6] of integer;
  54.    b_bez: array[0..6] of string;
  55.  
  56. (*********************************************************************)
  57. function arccos(x: real): real;
  58. begin
  59.      (* SEHR grobe Naeherung *)
  60.      arccos := -0.5*pi*x + pi/2;
  61. end;
  62.  
  63. (*********************************************************************)
  64. function min(x,y: real): real;
  65. begin
  66.      if x<y then
  67.         min:=x
  68.      else
  69.         min:=y;
  70. end;
  71.  
  72. (*********************************************************************)
  73. function maximum(x,y: real): real;
  74. begin
  75.      if x>y then
  76.         maximum:=x
  77.      else
  78.         maximum:=y;
  79. end;
  80.  
  81. (*********************************************************************)
  82. procedure balance_init;
  83. var
  84.    i: integer;
  85. begin
  86.      (* Grenzen fuer Alpha *)
  87.      b_alpha_l[0] := -30.0;   b_alpha_u[0] :=  30.0;
  88.      b_alpha_l[1] :=   0.0;   b_alpha_u[1] :=  60.0;
  89.      b_alpha_l[2] :=  30.0;   b_alpha_u[2] :=  90.0;
  90.      b_alpha_l[3] :=  60.0;   b_alpha_u[3] := 120.0;
  91.      b_alpha_l[4] :=  90.0;   b_alpha_u[4] := 150.0;
  92.      b_alpha_l[5] := 120.0;   b_alpha_u[5] := 180.0;
  93.      b_alpha_l[6] := 150.0;   b_alpha_u[6] := 210.0;
  94.  
  95.      for i:=0 to 6 do begin
  96.          b_alpha_l[i]:=b_alpha_l[i]*pi/180.0;
  97.          b_alpha_u[i]:=b_alpha_u[i]*pi/180.0;
  98.      end;
  99.  
  100.      (* Grenzen fuer alphaDot *)
  101.      b_alphaDot_l[0] := -5.0;   b_alphaDot_u[0] := -1.0;
  102.      b_alphaDot_l[1] := -3.0;   b_alphaDot_u[1] := -1.0;
  103.      b_alphaDot_l[2] := -2.0;   b_alphaDot_u[2] :=  0.0;
  104.      b_alphaDot_l[3] := -1.0;   b_alphaDot_u[3] :=  1.0;
  105.      b_alphaDot_l[4] :=  0.0;   b_alphaDot_u[4] :=  2.0;
  106.      b_alphaDot_l[5] :=  1.0;   b_alphaDot_u[5] :=  3.0;
  107.      b_alphaDot_l[6] :=  1.0;   b_alphaDot_u[6] :=  5.0;
  108.  
  109.      (* Regeln definieren *)
  110.      b_rule_alpha[0]:=PM; b_rule_alphaDot[0]:=ZE; b_rule_a[0]:=PM;
  111.      b_rule_alpha[1]:=PS; b_rule_alphaDot[1]:=PS; b_rule_a[1]:=PS;
  112.      b_rule_alpha[2]:=PS; b_rule_alphaDot[2]:=NS; b_rule_a[2]:=ZE;
  113.      b_rule_alpha[3]:=NM; b_rule_alphaDot[3]:=ZE; b_rule_a[3]:=NM;
  114.      b_rule_alpha[4]:=NS; b_rule_alphaDot[4]:=NS; b_rule_a[4]:=NS;
  115.      b_rule_alpha[5]:=NS; b_rule_alphaDot[5]:=PS; b_rule_a[5]:=ZE;
  116.      b_rule_alpha[6]:=ZE; b_rule_alphaDot[6]:=ZE; b_rule_a[6]:=ZE;
  117.  
  118.      (* Bezeichnungen fuer Indizes *)
  119.      b_bez[0]:='NB';
  120.      b_bez[1]:='NM';
  121.      b_bez[2]:='NS';
  122.      b_bez[3]:='ZE';
  123.      b_bez[4]:='PS';
  124.      b_bez[5]:='PM';
  125.      b_bez[6]:='PB';
  126. end;
  127.  
  128. (*********************************************************************)
  129. function balance(alpha,alphadot: real): real;
  130. var
  131.    b_alpha: array[0..6] of real;
  132.    b_alphaDot: array[0..6] of real;
  133.    b_Fa: array[0..6] of real;
  134.    max,mid: real;
  135.    i: integer;
  136.  
  137. begin
  138.      (* Gewichte von alpha ausrechnen *)
  139.      for i:=0 to 6 do begin
  140.          if (b_alpha_u[i] > alpha) and (alpha > b_alpha_l[i]) then begin
  141.             mid := (b_alpha_u[i]+b_alpha_l[i])/2;
  142.             if alpha>mid then begin
  143.                (* linke Haelfte *)
  144.                b_alpha[i] := 1-(alpha-mid)/(b_alpha_u[i]-mid);
  145.             end else begin
  146.                (* rechte Haelfte *)
  147.                b_alpha[i] := (alpha-b_alpha_l[i])/(mid-b_alpha_l[i]);
  148.             end;
  149.          end else begin
  150.              b_alpha[i]:=0.0;
  151.          end;
  152.      end;
  153.  
  154.      (* Gewichte von alphaDot ausrechnen *)
  155.      for i:=0 to 6 do begin
  156.          if (b_alphaDot_u[i] > alphaDot)
  157.             and (alphaDot > b_alphaDot_l[i]) then begin
  158.             mid := (b_alphaDot_u[i]+b_alphaDot_l[i])/2;
  159.             if alphaDot>mid then begin
  160.                (* rechte Haelfte *)
  161.                b_alphaDot[i] := 1.0-(alphaDot-mid)/(b_alphaDot_u[i]-mid);
  162.             end else begin
  163.                (* linke Haelfte *)
  164.                b_alphaDot[i] :=
  165.                    (alphaDot-b_alphaDot_l[i])/(mid-b_alphaDot_l[i]);
  166.             end;
  167.          end else begin
  168.              b_alphaDot[i]:=0.0;
  169.          end;
  170.      end;
  171.  
  172.      (* Regeln anwenden *)
  173.      for i:=0 to 6 do begin
  174.          b_Fa[i] := 0.0;
  175.      end;
  176.      for i:=0 to 6 do begin
  177.             b_Fa[b_rule_a[i]] := maximum(b_Fa[b_rule_a[i]],
  178.                                   min(b_alpha[b_rule_alpha[i]],
  179.                                       b_alphaDot[b_rule_alphaDot[i]]));
  180.      end;
  181.  
  182.      (* groessten b_Fa suchen *)
  183.      max:=0.0;
  184.      for i:=0 to 6 do begin
  185.          max:=max + (i-3)*b_Fa[i];
  186.      end;
  187.  
  188.      (* alpha ausgeben *)
  189.      gotoxy(58,3); write('alpha:');
  190.      for i:=0 to 6 do begin
  191.          gotoxy(58,4+i);
  192.          write(b_bez[i],': ',b_alpha[i]:5:3);
  193.      end;
  194.  
  195.      (* alphaDot ausgeben *)
  196.      gotoxy(70,3); write('alphaDot:');
  197.      for i:=0 to 6 do begin
  198.          gotoxy(70,4+i);
  199.          write(b_bez[i],': ',b_alphaDot[i]:5:3);
  200.      end;
  201.  
  202.      (* b_Fa ausgeben *)
  203.      gotoxy(65,13); write('b_Fa:');
  204.      for i:=0 to 6 do begin
  205.          gotoxy(65,14+i);
  206.          write(b_bez[i],': ',b_Fa[i]:5:3);
  207.      end;
  208.  
  209.      (* max ausgeben *)
  210.      gotoxy(65,22);
  211.      write('max=',max:6:3);
  212.  
  213.      balance := -max*a0 + a0_cheat;
  214. end;
  215.  
  216. (*********************************************************************)
  217. begin
  218.      clrscr;
  219.      for n:=0 to 180 do begin
  220.          alpha:=n*pi/180.0;
  221.          kx:=Xcenter+round(Xscale*cos(alpha));
  222.          ky:=Ycenter-round(Yscale*sin(alpha));
  223.          gotoxy(kx,ky);
  224.          write('.');
  225.      end;
  226.      gotoxy(Xcenter,Ycenter);
  227.      write('+');
  228.  
  229.      balance_init;
  230.  
  231.      n        := 1;
  232.      alphaDot := alphaDot0;
  233.      alpha    := alpha0;
  234.      Fa       := m*balance(alpha,alphaDot);
  235.      Fg       := m*g;
  236.  
  237.      while (alpha>=0) and (alpha<=pi) do begin
  238.            gotoxy(1,1);
  239.            write(n:4,': ');
  240.            write('alpha = ',(alpha*180/pi):7:3,'ø ');
  241.            write('alphaDot = ',alphaDot:6:3,' ');
  242.            write('deltaAlpha = ',(deltaAlpha*180/pi):6:3,'ø ');
  243.            write('Fa = ',Fa:6:3,' ');
  244.            write('                               ');
  245.            writeln;
  246.  
  247.            gotoxy(kx,ky); write('.');
  248.            kx:=Xcenter+round(Xscale*cos(alpha));
  249.            ky:=Ycenter-round(Yscale*sin(alpha));
  250.            gotoxy(kx,ky); write('@');
  251.  
  252.            (* Tastaturbehandlung *)
  253.            if keypressed then begin
  254.               c:=readkey;
  255.               if byte(c)=0 then begin
  256.                  c:=readkey;
  257.                  case byte(c) of
  258.                    77: begin
  259.                             alphaDot:=alphaDot - Bumper_alphaDot;
  260.                             alpha:=alpha-Bumper_alpha;
  261.                        end;
  262.                    75: begin
  263.                             alphaDot:=alphaDot + Bumper_alphaDot;
  264.                             alpha:=alpha+Bumper_alpha;
  265.                        end;
  266.                  end;
  267.               end else begin
  268.                  if c='p' then begin
  269.                     while not keypressed do;
  270.                  end;
  271.               end;
  272.            end;
  273.  
  274.            Fa := m*balance(alpha,alphaDot);
  275.  
  276.            (* Pendel-Verhalten nach Ch. Ziegaus *)
  277.            Fres       := sqrt(sqr(Fg) + sqr(Fa));
  278.            gamma      := alpha - arccos(Fa/Fres);
  279.            Fz         := sin(gamma) * Fres;
  280.            deltaAlpha := Fz/(2.0*m*l)*sqr(deltaT) + alphaDot*deltaT;
  281.            alphaDot   := Fz/(m*l)*deltaT + alphaDot;
  282.            alpha      := alpha + deltaAlpha;
  283.  
  284.            n          := n+1;
  285.  
  286.            (* while not keypressed do;*)
  287.      end;
  288.  
  289.      gotoxy(1,25);
  290.      write('CR:');
  291.      readln;
  292. end.
  293.