#include#include /* terrible hacky little simulator for 2nd order system (mass, spring, friction), integrated using euler method. Simple, and good enough to see some basic behavior. */ /* use this function for an outside disturbance force on the mass */ float drive_force (float t) /* can use to model disturbance force*/ { return (0.0); } /* use this for time-varying desired position */ float pos_desired (float t) /* time-dependent position and vel. */ { return(10.0); } /* use this for time-varying desired velocity*/ float vel_desired (float t) { return(0.0); } main(int argc, char *argv[]) { FILE *fp, *fpout, *flog; float accel, vel, pos, Pgain, Igain, Dgain, mass, frict, spring; float dt, t, time, int_decay, dv; float force, input, error, last_error, integrator, des_pos; float pos0, vel0; /* initial conds */ int i, ticks; if ((fp = fopen(argv[1], "r")) == NULL) {perror(argv[1]); exit(1);} /* input file on command line: e.g ./a.out in */ /* of course could read in the name of the output file too... but...*/ if ((fpout = fopen("out", "w")) == NULL) {perror("out"); exit(1);} /* ready for gnuplot, eg. plot "out" with linesp */ if ((flog = fopen("log", "w")) == NULL) {perror("log"); exit(1);} /* for interm. results, debugging, etc */ fscanf(fp, "%f %f %f %f %f %f %f %f %f %f %f %f", &mass, &frict, &spring, &pos, &vel, &Pgain, &Igain, &Dgain, &dt, &time, &int_decay, &des_pos); /* read in various system parameters (mass, frict, spring), init. conds (pos, vel), control systems gains (PID), time delta, total time, integrator decay (hint) (or maybe for you a length of time over which to integrate, or maybe nothing if you integrate over all past time, and a desired position (time-independent). */ fprintf(flog, "%f %f %f %f %f %f %f %f %f %f %f %f\n", mass, frict, spring, pos, vel, Pgain, Igain, Dgain, dt, time, int_decay, des_pos); ticks = time/dt; /* how many iterations */ t = 0.0; error = last_error = 0.0; /* for derivative of error calculation (hint */ integrator = 0.0; /* for my integral controller (hint) */ input = 0.0; /* output of my controller, a force to be input to system */ /*OK we're initialized */ for (i=0; i LESS_THAN ticks; i++) /* Replace with a real "less-than" sign... I can't use the right symbol here in the .html file, sorry! */ { /* your controller goes here! it has inputs like position, velocity, desired position, and it computes the variable input, which is force applied to the system. Mine was a four-liner*/ /* simple-minded simulation begins here.... */ force = input + drive_force(t) - spring*pos - frict*fabs(vel); /* add up all the forces acting on the mass, including some exterior disturbance and your control input */ /* fprintf (flog,"%f %f %f %f \n", t, error, input, force); */ /* sample log output for debugging etc */ accel = force/mass; dv = accel*dt; /* euler integration once */ vel += dv; pos += vel*dt + (0.5)* accel*dt*dt; /* euler int. twice */ fprintf(fpout,"%f %f\n",t,pos); /* this file is input to gnuplot */ t += dt; /* the tick of time tocks on....tempus fugit */ } fclose(fp); fclose(fpout); fclose(flog); } /* at this point you can go to gnuplot and plot your output file.*/