/* 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. */
  /* use this for time-varying desired velocity*/
  float vel_desired (float t)
  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). */
  "%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 
      /* 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 */
  /* at this point you can go to gnuplot and plot your output file.*/