home *** CD-ROM | disk | FTP | other *** search
/ AMIGA PD 1 / AMIGA-PD-1.iso / Meeting_Pearls_II / html / sw / nbsd / FuzzyPendel / pendel.c < prev    next >
C/C++ Source or Header  |  1994-06-28  |  4KB  |  167 lines
  1. #include <sys/types.h>
  2. #include <unistd.h>
  3. #include <fcntl.h>
  4. #include <curses.h>
  5. #include <math.h>
  6. #include <signal.h>
  7. #include "balance.h"
  8.  
  9. #define pi               M_PI
  10. #define g                9.81
  11.  
  12. #define LOGFILE          "pendel.log"
  13. #define m                1.0
  14. #define l                1.0
  15. #define deltaT           0.005
  16. #define alpha0           25.0*(pi/180.0)
  17. #define alphaDot0        0.0
  18. #define a0               20.0
  19.  
  20. #define Bumper_alphaDot  0.5
  21. #define Bumper_alpha     10.0*(pi/180.0)
  22.  
  23. #define Xcenter          30
  24. #define Ycenter          23
  25. #define Xscale           25.0
  26. #define Yscale           20.0
  27.  
  28. double alpha,Fa,Fres,Fz,alphaDot,deltaAlpha,rho,Fg,beta;
  29. FILE *Log;
  30.  
  31. /*************************************************************************/
  32. void Delay(void)
  33. {
  34.     int flags;
  35.     fcntl(0,F_GETFL,&flags);
  36.     flags&=~O_NDELAY;
  37.     fcntl(0,F_SETFL,flags);
  38. }
  39.  
  40. void Nodelay(void)             { fcntl(0,F_SETFL,O_NDELAY); }
  41. double sqr(double x)           { return x*x; }
  42. void gotoxy(int x, int y)      { move(y-1,x); }
  43. int round(double x)            { return (int)(x); }
  44. double Max(double x, double y) { return (x>y)?x:y; }
  45. double Min(double x, double y) { return (x<y)?x:y; }
  46. double arccos(double x)        { return acos(x); }
  47.  
  48. /*************************************************************************/
  49. void exitfn(int sig)
  50. {
  51.     gotoxy(1,25);
  52.     printw("CR: ");
  53.     refresh();
  54.  
  55.     nocbreak();
  56.     echo();
  57.     Delay();
  58.     getch();
  59.  
  60.     endwin();
  61.     if (Log) fclose(Log);
  62.  
  63.     exit(0);
  64. }
  65.  
  66. /*************************************************************************/
  67. void main(void)
  68. {
  69.     int ch;
  70.     int cnt,n;
  71.     int kx,ky;
  72.     WINDOW *win;
  73.  
  74.     signal(SIGINT, exitfn);
  75.     signal(SIGQUIT,exitfn);
  76.     signal(SIGTERM,exitfn);
  77.     
  78.     if(!(Log=fopen(LOGFILE,"w"))){
  79.     perror("Can't open log-file '" LOGFILE "'");
  80.     exit (1);
  81.     }
  82.     
  83.     win=initscr();
  84.     cbreak();
  85.     noecho();
  86.     Nodelay();
  87.     
  88.     for(n=0;n<=180;n++){
  89.     alpha=n*pi/180.0;
  90.     kx=Xcenter+round(Xscale*cos(alpha));
  91.     ky=Ycenter-round(Yscale*sin(alpha));
  92.      
  93.     gotoxy(kx,ky);
  94.     printw(".");
  95.     }
  96.     gotoxy(Xcenter,Ycenter);
  97.     printw("+");
  98.     refresh();
  99.  
  100.     balance_init();
  101.     refresh();
  102.     
  103.     n        = 1;
  104.     cnt      = 1;
  105.     alphaDot = alphaDot0;
  106.     alpha    = alpha0;
  107.     Fa       = m*balance(alpha,alphaDot)*a0;
  108.     Fg       = m*g;
  109.     
  110.     while (alpha>=0.0 && alpha<=pi){
  111.     gotoxy(1,1);
  112.     printw("%4d: ",n);
  113.     printw("alpha = %7.3f ",(alpha*180.0/pi));
  114.     printw("alphaDot = %6.3f ",alphaDot);
  115.     printw("deltaAlpha = %6.3f ",deltaAlpha);
  116.     printw("Fa = %6.3f ",Fa);
  117.     printw("                                  ");
  118.     fprintf(Log,"%d %6.3f\n",n,cos(alpha));
  119.     
  120.     gotoxy(kx,ky); addch('.');
  121.     kx=Xcenter+round(Xscale*cos(alpha));
  122.     ky=Ycenter-round(Yscale*sin(alpha));
  123.     gotoxy(kx,ky); addch('@');
  124.     refresh();
  125.  
  126.     /* Tastaturbehandlung */
  127.     if((ch=getch())!=ERR){
  128.         switch(ch){
  129.           case 68:
  130.         /* Bump left */
  131.         alphaDot = alphaDot + Bumper_alphaDot;
  132.         alpha    = alpha    + Bumper_alpha;
  133.         break;
  134.           case 67:
  135.         /* Bump right */
  136.         alphaDot = alphaDot - Bumper_alphaDot;
  137.         alpha    = alpha    - Bumper_alpha;
  138.         break;
  139.           case 70:
  140.         Delay();
  141.         while(getch()==ERR);
  142.         Nodelay();
  143.         break;
  144.           case 'p':
  145.         alpha=2*pi;
  146.         break;
  147.         }
  148.     }
  149.     
  150.     /* f2: Ausgleichen der Pendelbewegung */
  151.     Fa = m*balance(alpha,alphaDot)*a0;
  152.  
  153.     /* f1: Pendel-Verhalten nach Ch. Ziegaus */
  154.     Fres       = sqrt(sqr(Fg) + sqr(Fa));
  155.     rho        = alpha - arccos(Fa/Fres);
  156.     Fz         = sin(rho) * Fres;
  157.     deltaAlpha = Fz/(2.0*m*l)*sqr(deltaT) + alphaDot*deltaT;
  158.     alphaDot   = Fz/(m*l)*deltaT + alphaDot;
  159.     alpha      = alpha + deltaAlpha;
  160.     
  161.     n          = n+1;
  162.     }
  163.     
  164.     exitfn(0);
  165.     /*NOTREACHED*/
  166. }
  167.