mems_piecewise_seek.c

disksim是一个非常优秀的磁盘仿真工具

 *  initial velocity is equal to the final velocity
 *  (i.e. there are no turnarounds necessary).  it uses an
 *  iterative algorithm to find the optimal switchpoint,
 *  attempting to balance the energy in the acceleration
 *  phase with the energy in the deceleration phase.
 *
 *  please see below for a complete explanation of how it works.
 *
 *  inputs:
 *    double start_offset_nm - the starting offset
 *    double end_offset_nm - the end offset
 *    double spring_factor - spring factor of the system
 *    double acceleration_nm_s_s - actuator acceleration
 *    double length_nm - the length of the sled
 *    double velocity_nm_s - the initial and final velocity
 *
 *  outputs:
 *    double - the seek time from start to end
 *
 *  modifies:
 *    nothing
 *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-*/

double
find_seek_time_piecewise(double start_offset_nm, double end_offset_nm,
			 double spring_factor, double acceleration_nm_s_s,
			 double length_nm, double velocity_nm_s) {

  double dist_nm;                      /*  distance of this seek  */
  double phase1_dist_nm;               /*  distance of the first phase  */
  double phase2_dist_nm;               /*  distance of the second phase  */
  double phase1_chunk_length_nm;       /*  length of chunks in the first phase  */
  double phase2_chunk_length_nm;       /*  length of chunks in the second phase  */
  double phase1_acceleration_nm_s_s;   /*  actuator acceleration in the first phase  */
  double phase2_acceleration_nm_s_s;   /*  actuator acceleration in the second phase  */
  double x[2 * NUM_CHUNKS + 1];        /*  position values for each chunk of the seek  */
  double v[2 * NUM_CHUNKS + 1];        /*  velocity values for each chunk of the seek  */
  double a[2 * NUM_CHUNKS + 1];        /*  adjusted acceleration values for each chunk  */
  double t[2 * NUM_CHUNKS + 1];        /*  times at each x point  */

  double x_switch, v_switch, a_switch, t_switch;  /*  position, velocity, acceleration, and time values for the switch point  */
  double v_diff, x_delta;

  double min_v_diff;
  double min_v_diff_time = 0.0;

  double total_seek_time;

  int i, jj;
  int overall_seek_direction;
  int phase1_actuator_direction, phase2_actuator_direction;

  min_v_diff = 666.0;  /*  this should be bigger than 100 percent  */
  
  if (_VERBOSE_) {
    fprintf(__OUTPUTFILE__, "find_seek_time_piecewise:: ** entry - start_offset_nm = %f, end_offset_nm = %f\n",
	    start_offset_nm, end_offset_nm);
  }

  /*  find seek distance and overall direction  */
  dist_nm = find_dist_nm(start_offset_nm, end_offset_nm);
  overall_seek_direction = find_overall_seek_direction(start_offset_nm, end_offset_nm);

  /*  set initial x position and set switch point to be midpoint of seek  */
  x[0] = start_offset_nm;
  x[SWITCH_POINT] = find_switch_offset_from_dist(start_offset_nm, end_offset_nm,
						 dist_nm, overall_seek_direction);

  /*  this is the loop which iteratively tries to find the optimal switch point.  during each iteration
      the loop does the following:

      find the x values for each chunk.
      find the adjusted acceleration at each point.
      start at the start point and find the v and t values
         for each point in the first phase, including the switch point.
      start at the end point and find the v and t values
         for each point in the second phase, again including the switch point.
      
      if the switch point we picked initially were the optimal one, then the
      velocity at the switch point from each phase should be equal, by conservation
      of energy.  if they're not, then we need to adjust the switch point toward the
      end that had more energy, i.e. had the highest velocity at the switch point.
      the loop below iterates a fixed number of times, adjusting the switch point toward
      the start or end point by some percentage of the seek distance.

      at compile time, you could lower the number of MEMS_SWITCH_ITERATIONS to reduce the runtime, at the
      cost of accuracy.  if the phases of the calculation are unbalanced, then the sled will not be at the
      correct velocity when it arrives at the destination.  you could also reduce the number of chunks each
      phase is broken into.  this leads to discontinuities in the seek curve.  this is due to the way it
      decides when the springs switch from helping the seek to hurting the seek.  when a chunk straddles the
      middle of the square, the springs will be changing from helping to hurting.  however, we can only choose
      one or the other for each chunk, so for part of it the spring factor will be incorrectly determined.
      the smaller the chunks are, the less significant this inaccuracy becomes.
  */
  
  for (jj = 1; jj < MEMS_SWITCH_ITERATIONS; jj++) {
    
    /*  find the length of the two phases  */
    phase1_dist_nm = find_dist_nm(start_offset_nm, x[SWITCH_POINT]);
    phase2_dist_nm = find_dist_nm(x[SWITCH_POINT], end_offset_nm);

    /*  find the length of the chunks  */
    phase1_chunk_length_nm = phase1_dist_nm / NUM_CHUNKS;
    phase2_chunk_length_nm = phase2_dist_nm / NUM_CHUNKS;

    if (_VERBOSE_) {
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  phase1_dist_nm = %f, phase1_chunk_length_nm = %f\n",
	      phase1_dist_nm, phase1_chunk_length_nm);
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  phase2_dist_nm = %f, phase2_chunk_length_nm = %f\n",
	      phase2_dist_nm, phase2_chunk_length_nm);
    }

    if (_VERBOSE_) fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  ** figuring out phase 1 x values\n");

    /*  find the endpoints of all the chunks of the first phase  */
    for (i = 1; i < SWITCH_POINT; i++) {
      if (overall_seek_direction == OVERALL_POSITIVE)
	x[i] = x[i-1] + phase1_chunk_length_nm;
      else
	x[i] = x[i-1] - phase1_chunk_length_nm;
    }

    if (_VERBOSE_) fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  ** figuring out phase 2 x values\n");

    /*  find the endpoints of all the chunks of the second phase  */
    for (i = SWITCH_POINT + 1; i <= 2*NUM_CHUNKS; i++) {
      if (overall_seek_direction == OVERALL_POSITIVE)
	x[i] = x[i-1] + phase2_chunk_length_nm;
      else
	x[i] = x[i-1] - phase2_chunk_length_nm;
    }  

    /*  the absolute value of the actuator acceleration is equal for both phases...  */
    phase1_acceleration_nm_s_s = acceleration_nm_s_s;
    phase2_acceleration_nm_s_s = acceleration_nm_s_s;

    /*  ...but the directions will be different  */
    if (overall_seek_direction == OVERALL_POSITIVE) {
      phase1_actuator_direction = OVERALL_POSITIVE;
      phase2_actuator_direction = OVERALL_NEGATIVE;
    } else {
      phase1_actuator_direction = OVERALL_NEGATIVE;
      phase2_actuator_direction = OVERALL_POSITIVE;
    }

    if (_VERBOSE_) fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  ** adjusting phase 1 accelerations\n");

    /*  find the acceleration in each chunk of the first phase.  this takes into account the adjustment
	for the spring forces.  */
    for (i = 1; i <= SWITCH_POINT; i++) {
      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  i = %d, x[i-1] = %f, x[i] = %f\n",
		i, x[i-1], x[i]);
      }
      a[i] = adjust_accel(phase1_acceleration_nm_s_s, spring_factor,
			  x[i-1], x[i],
			  length_nm, phase1_actuator_direction);
    }

    if (_VERBOSE_) fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  ** adjusting phase 2 accelerations\n");

    /*  find the acceleration in each chunk of the second phase.  this takes into account the adjustment
	for the spring forces.  */
    for (i = SWITCH_POINT + 1; i <= 2*NUM_CHUNKS; i++) {
      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  i = %d, x[i-1] = %f, x[i] = %f\n",
		i, x[i-1], x[i]);
      }
      a[i] = adjust_accel(phase2_acceleration_nm_s_s, spring_factor,
			  x[i-1], x[i],
			  length_nm, phase2_actuator_direction);
    }

    /*  start at time 0.0 and with the correct initial velocity  */
    t[0] = 0.0;
    v[0] = velocity_nm_s;

    if (_VERBOSE_) fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  ** finding phase 1 t's and v's\n");

    /*  find the time and velocity at each point in the first phase  */
    for (i = 1; i <= SWITCH_POINT; i++) {
      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  v[i-1] = %f, a[i] = %f, t[i-1] = %f, x[i-1] = %f, x[i] = %f\n",
		v[i-1], a[i], t[i-1], x[i-1], x[i]);
      }
      t[i] = (
	      -(v[i-1] - (a[i] * t[i-1]))
	      + sqrt(
		     (v[i-1] * v[i-1]) + ((2.0 * a[i]) * fabs((x[i] - x[i-1])))
		     )
	      ) / (a[i]);
    
      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  t[%d] = %f\n", i, t[i]);
      }

      v[i] = (
	      v[i-1] + (a[i] * (t[i] - t[i-1]))
	      );
    
      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  v[%d] = %f\n", i, v[i]);    
      }
    }

    /*  now we've found half of the seek time  */
    total_seek_time = t[SWITCH_POINT];

    /*  save the values at the switch point since they're about to be over-written by
	the calculation for the second phase  */
    x_switch = x[SWITCH_POINT];
    v_switch = v[SWITCH_POINT];
    a_switch = a[SWITCH_POINT];
    t_switch = t[SWITCH_POINT];

    /*  start the second phase at time 0.0 and at the initial velocity  */
    t[2*NUM_CHUNKS] = 0.0;
    v[2*NUM_CHUNKS] = velocity_nm_s;

    if (_VERBOSE_) fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  ** finding phase 2 t's and v's\n");

    /*  find the time and velocity at each point in the second phase  */
    for (i = 2*NUM_CHUNKS - 1; i >= SWITCH_POINT; i--) {
      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  v[i+1] = %f, a[i] = %f, t[i+1] = %f, x[i+1] = %f, x[i] = %f\n",
		v[i+1], a[i], t[i+1], x[i+1], x[i]);
      }
      t[i] = (
	      -(v[i+1] - (a[i] * t[i+1]))
	      + sqrt(
		     (v[i+1] * v[i+1]) + ((2.0 * a[i]) * fabs((x[i+1] - x[i])))
		     )
	      ) / (a[i]);
    
      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  t[%d] = %f\n", i, t[i]);
      }

      v[i] = (
	      v[i+1] + (a[i] * (t[i] - t[i+1]))
	      );

      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  v[%d] = %f\n", i, v[i]);
      }
    }

    /*  now we've found the total seek time  */
    total_seek_time += t[SWITCH_POINT];    

    /*  print out all of the x, t, a, and v values that we've found  */
    if (_VERBOSE_) {
      for (i = 0; i <= 2 * NUM_CHUNKS; i++) {
	if (i == SWITCH_POINT) {
	  fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  x[%d] = %f, a[%d] = %f, v[%d] = %f, t[%d] = %f  ** SWITCH_POINT **\n",
		  i, x[i], i, a[i], i, v[i], i, t[i]);
	} else {
	  fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  x[%d] = %f, a[%d] = %f, v[%d] = %f, t[%d] = %f\n",
		  i, x[i], i, a[i], i, v[i], i, t[i]);
	}
      }

      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  total_seek_time = %f\n", fabs(total_seek_time));
    
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  x_switch = %f, x[SWITCH_POINT] = %f, diff = %f\n",
	      x_switch, x[SWITCH_POINT], ((x_switch / x[SWITCH_POINT]) * 100.0) - 100.0);
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  v_switch = %f, v[SWITCH_POINT] = %f, diff = %f\n",
	      v_switch, v[SWITCH_POINT], ((v_switch / v[SWITCH_POINT]) * 100.0) - 100.0);
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  a_switch = %f, a[SWITCH_POINT] = %f, diff = %f\n",
	      a_switch, a[SWITCH_POINT], ((a_switch / a[SWITCH_POINT]) * 100.0) - 100.0);
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  t_switch = %f, t[SWITCH_POINT] = %f, diff = %f\n",
	      t_switch, t[SWITCH_POINT], ((t_switch / t[SWITCH_POINT]) * 100.0) - 100.0);
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  trying to find new switch point.  jj = %d\n", jj);
    } 

    /*  look at the difference in the velocities calculated in the two phases  */
    v_diff = ((v_switch / v[SWITCH_POINT]) * 100.0) - 100.0;

    /*  if we're not taking the springs into account, then we don't need to iterate anymore.  */
    if (spring_factor == 0.0) {
      min_v_diff_time = total_seek_time;
      break;
    } else {
      /*  otherwise, come up with the amount by which we want to shift the switchpoint - a percentage
          of the seek distance minus a diminishing function of the iteration number  */
      x_delta = (MEMS_SHIFT_PERCENTAGE - (1.0 / ((MEMS_SWITCH_ITERATIONS - jj) * 100.0)));
    }

    if (v_switch > v[SWITCH_POINT]) {
      /*  the first phase ended up with more energy  */
      if (overall_seek_direction == OVERALL_POSITIVE) {
	x_delta = -x_delta;
      }      
    } else {
      if (overall_seek_direction == OVERALL_NEGATIVE) {
	x_delta = -x_delta;
      }      
    }

    /*  adjust the switch point  */
    x[SWITCH_POINT] += x_delta * dist_nm;
  
    if (_VERBOSE_) {
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  shifting x_switch. x_delta = %f, x_delta_nm = %f, v_diff = %f\n",
	      x_delta, (x_delta * dist_nm), v_diff);
    
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  x_switch = %f, x[SWITCH_POINT] = %f, diff = %f\n",
	      x_switch, x[SWITCH_POINT], ((x_switch / x[SWITCH_POINT]) * 100.0) - 100.0);
    }

    x[0] = start_offset_nm;

    if (_VERBOSE_) {
      fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  min_v_diff = %f, v_diff = %f\n",
	      min_v_diff, v_diff);
    }

    /*  save the minimum velocity difference and the seek time which got me that v_diff  */
    if (fabs(v_diff) < min_v_diff) {
      min_v_diff = fabs(v_diff);
      min_v_diff_time = total_seek_time;
      if (_VERBOSE_) {
	fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  updating min_v_diff\n");
	if (min_v_diff == 0.0) fprintf(__OUTPUTFILE__, "find_seek_time_piecewise::  augh!!!  min_v_diff is zero!\n");
      }
    }
  }

  /*  return the seek time from the iteration which gave me the smallest v_diff  */
  return fabs(min_v_diff_time);
}