Here is solution to your proplem that has been passed down by FIRST programmers for many moons (also on the InnovationFirst WEB site). This first order filter will limit the rate of change of the inputs.
Drive_yFiltCoef CON 50 'First Order Filter Coefficient = CON/200
Drive_xFiltCoef CON 20 'Adjust these values for improved control. Trial and Error.
Drive_yFilt VAR byte 'First Order Filter output for the Y-axis Joystick Input
Drive_xFilt VAR byte 'First Order Filter output for the X-axis Joystick Input
'**First order filter to limit rate of change of drive motors**
Drive_yFilt = (Drive_yFilt + ((256 + p1_Y - Drive_yFilt) * Drive_yFiltCoef / 200) - (256 * Drive_yFiltCoef / 200)) Min 0 Max 254
Drive_xFilt = (Drive_xFilt + ((256 + p1_X - Drive_xFilt) * Drive_xFiltCoef / 200) - (256 * Drive_xFiltCoef / 200)) Min 0 Max 254
