/* BFLIGHT.C */ /* A dynamic model of the blimp in flight. */ /* The program reads a user specified data file. */ /* The data file contains locations and magnitudes of the forces acting */ /* on the blimp. */ #include #include #include #include #include #define PI 3.14159 #define THRUST_STEP 0.4 #define T_ANGLE_STEP 5.0 * PI / 180.0 #define DELTA_TIME 0.1 #define NUMPOINTS 24 #define HEAD 5 typedef struct { double xl,yl, xd,yd, xt,yt, ml,mw, mass; } DATASET; /* The format of the data file. */ DATASET _d; int _cx,_cy; /* coordinates of the center of the screen. */ double _scx,_scy; /* scale factor for display (pixels/meter). */ struct { double r, angle; } blimp[NUMPOINTS]; /* used to display the blimp(ellipse approximation). */ char _string[256]; /* FUNCTION PROTOTYPES */ void plot_blimp(double,double,double,double,double,double,double); void find_phi(double,double,double,double,double *,double *); void draw_vect(double,double,double,double,double); /* FUNCTION DECLARATIONS */ void draw_vect(x,y,mag,theta,angle) /* Draw a vector on the screen. */ double x,y, /* coordinates of the tail (blimp relative). */ mag, /* magnitude of the vector. */ theta, /* blimp relative angle of the vector. */ angle; /* blimp angle relative to the screen. */ { int sx,sy; double r; r = sqrt(x * x + y * y); sx = _cx + (int)(_scx * r * cos(angle)); sy = _cy - (int)(_scy * r * sin(angle)); moveto(sx, sy); /* set the tail of the vector. */ /* draw the vector and arrowhead. */ linerel(_scx * mag * cos(angle + theta), -_scy * mag * sin(angle + theta)); linerel(HEAD * cos(theta+angle-0.75*PI), -HEAD * sin(theta+angle-0.75*PI)); } /* draw_vect */ void plot_blimp(angle,md,td,mt,tt,mv,tv) /* display the blimp and force vectors on the screen. */ double angle, /* angle of the blimp relative to the ground (screen). */ md,td, /* drag information. */ mt,tt, /* thrust information. */ mv,tv; /* velocity information. */ { double theta; int i; /* clear the graphics screen. */ cleardevice(); /* draw the velocity vector in red. */ setcolor(RED); draw_vect(0.0, 0.0, mv, tv, angle); /* draw the thrust vector in green. */ setcolor(GREEN); draw_vect(_d.xt, _d.yt, mt/5.0, tt, angle); /* draw the drag vector in blue. */ setcolor(BLUE); draw_vect(_d.xd, _d.yd, md/5.0, td, angle); /* draw the blimp in white. */ setcolor(WHITE); theta = blimp[0].angle + angle; moveto(_cx + _scx * blimp[0].r * cos(theta), _cy - _scy * (blimp[0].r * sin(theta) + _d.yl)); for(i=1; i < NUMPOINTS; i++) { theta = blimp[i].angle + angle; lineto(_cx + _scx * blimp[i].r * cos(theta), _cy - _scy * (blimp[i].r * sin(theta) + _d.yl)); } theta = blimp[0].angle + angle; lineto(_cx + _scx * blimp[0].r * cos(theta), _cy - _scy * (blimp[0].r * sin(theta) + _d.yl)); } /* plot_blimp */ void find_phi(td,tt,md,mt,p1,p2) /* find the angle of the blimp relative to the ground (screen). */ double td,tt, /* angles. */ md,mt, /* magnitudes. */ *p1,*p2; /* return variables. */ { double a1,a2,a3,b1,b2,a,b,c; double u1,u2,us; /* intermediate variables for the calculation of phi. */ a1 = (_d.xd*sin(td) + _d.yd*cos(td)) * md + (_d.xt*sin(tt) + _d.yt*cos(tt)) * mt; a2 = (_d.xl+_d.xd+_d.xt)*(md*sin(td)+mt*sin(tt))-(_d.yl+_d.yd+_d.yt)*(md*cos(td)+mt*cos(tt)); b1 = (_d.xl+_d.xd+_d.xt)*(_d.ml-_d.mw); b2 = (_d.yl+_d.yd+_d.yt)*(_d.ml-_d.mw) + _d.yl * _d.ml; /* valid for 1st and 4th quadrants only! */ a3 = a1 - a2; a = b1 * b1 + b2 * b2; b = 2.0 * b2 * a3; c = a3 * a3 - b1 * b1; us = b * b - 4 * a * c; if(us >= 0.0) { u1 = (-b + sqrt(us)) / (2.0 * a); u2 = (-b - sqrt(us)) / (2.0 * a); } else { u1 = -999.9; /* value for no solution. */ u2 = -999.9; } if(u1 >= -1.0 && u1 <= 1.0) *p1 = -asin(u1); else *p1 = -999.9; if(u2 >= -1.0 && u2 <= 1.0) *p2 = -asin(u2); else *p2 = -999.9; } /* find_phi */ main() { int i; double td,tt, md,mt, mv,tv, p1,p2; double w,w_old, ax,ay, vx,vx_old, vy,vy_old, dragx,dragy, kx_drag,ky_drag; double phi_valid, phi_valid_old, dt, alpha; char ch; int g_driver, g_mode , g_error; FILE* fhandle; double tng; printf("File to use as specification data: "); scanf("%s",_string); printf("\n"); if((fhandle = fopen(_string,"r")) == NULL) { printf("ERROR opening file -- %s\n",_string); exit(0); } /* initialize the graphics. */ detectgraph(&g_driver, &g_mode); if(g_driver < 0) { printf("No graphics hardware detected!\n"); exit(1); } initgraph(&g_driver,&g_mode,""); g_error = graphresult(); if(g_error < 0) { printf("Initgraph error: %s,\n",grapherrormsg(g_error)); exit(1); } /* read the blimp data from the file. */ fread(&(_d),sizeof(DATASET),1,fhandle); p1 = 0.0; p2 = 0.0; td = PI; tt = 0.0; md = 0.0; mt = 1.0; dt = DELTA_TIME; phi_valid = 0.0; phi_valid_old = 0.0; w_old = 0.0; vx_old = 0.0; vy_old = 0.0; kx_drag = 0.5 * 1.29 * 0.05 * PI; ky_drag = 0.5 * 1.29 * 0.07 * PI; _cx = getmaxx()/2; _cy = getmaxy()/2; _scx = (double)(getmaxx()) / 12.0; _scy = (double)(getmaxy()) / 12.0; for(i=0; i < NUMPOINTS; i++) { blimp[i].angle = (double)(i) * 2.0 * PI / NUMPOINTS; tng = blimp[i].angle; blimp[i].r = sqrt(1.0/(cos(tng)*cos(tng)/9.0+sin(tng)*sin(tng))); } while(1) { /* find the angle of the blimp relative to the ground (screen). */ /* once the angle is known all other unknowns can be found. */ find_phi(td,tt,md,mt,&p1,&p2); /* determine which phi is valid */ if((p1 == -999.9) && (p2 == -999.9)) phi_valid = phi_valid; else if(p1 == -999.9) phi_valid = p2; else if(p2 == -999.9) phi_valid = p1; else phi_valid = p1; /* valid phi is now variable phi_valid */ /* find the angular velocity of the blimp about the center of mass. */ w = (phi_valid - phi_valid_old) / dt; phi_valid_old = phi_valid; /* find the angular acceleration about the center of mass. */ alpha = (w - w_old) / dt; w_old = w; /* find the linear accelerations of the center of mass. */ ax = ((_d.ml-_d.mw)*sin(phi_valid) + md*cos(td) + mt * cos(tt)) / _d.mass; ay = ((_d.ml-_d.mw)*cos(phi_valid) + md*sin(td) + mt * sin(tt)) / _d.mass; /* find the linear velocities of the center of mass. */ vx = vx_old + ax * dt; vx_old = vx; vy = vy_old + ay * dt; vy_old = vy; /* find the magnitude and angle of the velocity for display purposes. */ if(vx < 0.0) if(vx > 0.00001 || vx < -0.00001) tv = atan(vy/vx) + PI; else if(vy < 0.0) tv = PI * 3.0 / 2.0; else tv = PI / 2.0; else if(vx > 0.00001 || vx < -0.00001) tv = atan(vy/vx); else if(vy < 0.0) tv = PI * 3.0 / 2.0; else tv = PI / 2.0; mv = sqrt(vx * vx + vy * vy); /* calculate the drag values and the vector form of the drag. */ dragx = kx_drag * vx * vx; if(vx > 0.0) dragx = -dragx; dragy = ky_drag * vy * vy; if(vy > 0.0) dragy = -dragy; md = sqrt(dragx*dragx + dragy*dragy); if(dragx < 0.0) if(dragx > 0.00001 || dragx < -0.00001) td = atan(dragy/dragx) + PI; else if(dragy < 0.0) td = PI * 3.0 / 2.0; else td = PI / 2.0; else if(dragx > 0.00001 || dragx < -0.00001) td = atan(dragy/dragx); else if(dragy < 0.0) td = PI * 3.0 / 2.0; else td = PI / 2.0; /* display the blimp and vectors on the screen. */ plot_blimp(phi_valid,md,td,mt,tt,mv,tv); /* read the keyboard and modify the trust vector accordingly. */ if(kbhit() != 0) { switch((ch = getch())) { case '7': mt += THRUST_STEP; break; case '1': mt -= THRUST_STEP; break; case '9': tt += T_ANGLE_STEP; if(tt > 2 * PI) tt -= 2 * PI; break; case '3': tt -= T_ANGLE_STEP; if(tt < 0.0) tt += 2 * PI; break; } /* switch */ if(ch == 'q' || ch == 'Q') break; } /* if(kbhit() != 1) */ } /* while(1) */ /* return to text mode. */ closegraph(); /* close the data file. */ fcloseall(); } /* main */