Incrementation Code not working

[benhulett]

We were working on our Incrementation code for our control system, it just makes sure we dont' make abrupt changes in our movements that could damage something....we had it working for the Y coordinates, but then we incorporated it into a function so both Y and X could call on it. That's where everything went wrong....The servos we were testing with would move in random directions without us controlling. Here is the code we wrote:

int Incrementation_Code(int motor_curr, int motor_prev, int padd, int max_step)
{
  int change = 0;
  int RetVal;

  change = motor_curr - motor_prev;

  if (change >= padd)
  {
    //Accelerate
	if (change > max_step)
    {
      RetVal = motor_prev + max_step;
      printf("Motor accelerated by MAX = %d.  ", max_step);
	}
    else
    {
      printf("Motor accelerated by %d.  ", change);
    }
	
	if ( RetVal > 255 )
	{
      RetVal = 255;
      printf("Detected excess of 255!  ");
    }

  }
  else if (change <= -padd)
  {
    //Decelerate
    if (change < -max_step)
    {
      RetVal = motor_prev - max_step;
      printf("Motor decelerated by MAX = %d.  ", max_step);
    }
    else
    {
      printf("Motor decelerated by %d.  ", change);
    }

    if (RetVal < 0)
    {
      RetVal = 0;
      printf("Detected below-zero condition!  ");
    }
  }
  else
  {
    //no change
    RetVal = motor_prev;
	printf("No change in motor output value.  ");
  }
  printf("\r\n");
  return RetVal;
}


int last_p1x_val=128;
int last_p1y_val=128;


void Default_Routine(void)
{

	int maximum_step=10;
	int padding=3;

 /*---------- Analog Inputs (Joysticks) to PWM Outputs-----------------------
  *--------------------------------------------------------------------------
  *   This maps the joystick axes to specific PWM outputs.    */

  p1_y = Incrementation_Code(p1_y, last_p1y_val, padding, maximum_step);
  last_p1y_val = p1_y;
  pwm01 = p1_y;
  pwm02 = p2_y;   
  pwm03 = p3_y;   
  pwm04 = p4_y;   
  p1_x = Incrementation_Code(p1_x, last_p1x_val, padding, maximum_step);
  last_p1x_val = p1_x;
  pwm05 = p1_x;  
  pwm06 = p2_x;   
  pwm07 = p3_x;   
  pwm08 = p4_x;   
  pwm09 = p1_wheel;
  pwm10 = p2_wheel;   
  pwm11 = p3_wheel;   
  pwm12 = p4_wheel;