Setpoint = map(analogRead(2), 0, 1023, 0, 255);
There is no clue, from reading this, exactly what is controlling the set point. Why not?
map() is a pretty expensive way to divide by 4.
double gap = abs(Setpoint-Input); //distance away from setpoint
if(gap<100)
{ //we're close to setpoint, use conservative tuning parameters
myPID.SetTunings(consKp, consKi, consKd);
}
else
{
//we're far from setpoint, use aggressive tuning parameters
myPID.SetTunings(aggKp, aggKi, aggKd);
}
You don't diddle with the tuning parameters based on how far off you are. You experiment to find tuning parameters, and use them. Consistently.
int TPS0 = analogRead(0);
int TPS1 = analogRead(1);
Again, there is no clue what you are reading from. Why not?
int TPS = TPS1 - TPS0;
TPS = map(TPS, -715, 910, 0, 255);
Input = TPS;
What purpose is TPS serving? None that I can see.