/*******************************************************************************
* FILE NAME: speed_control.c <FRC VERSION>
*
* DESCRIPTION:
*  This routine handles the ramping of the motor values to
*  the joystick input values, especially slowing down near 0.
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your 
*  project and replace it with a modified copy. 
********************************************************************************/

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "speed_control.h"


/****************************************************************
*   Function: speed_control
*   Purpose:  to ramp up speed gradually and slow down when
 switching between positive and negative.
*****************************************************************/

int speed_control(int input_speed, int *previous_speed, int rate)
{
int speed_diff;
speed_diff = input_speed - *previous_speed;

  /* Clamp the difference at the rate */
  if (speed_diff > rate)
    speed_diff = rate;
  else if (speed_diff < -rate)
    speed_diff = -rate;
  *previous_speed += speed_diff;

  /* Clamp at the limit */
  if (*previous_speed < -128)
    *previous_speed = -128;
  else if (*previous_speed > 127)
    *previous_speed = 127;

return *previous_speed;
}