float lastError = 0; float integrated = 0;float setPoint = 0;float error = 0;float Pterm = 0;float Iterm = 0;float Dterm = 0;// set PID gainsfloat kP = 120;float kI = 0;float kD = 190;// set max and min outputs int outMax = 255; int outMin = -255; float calcPid(float currentInput){ error = setPoint - currentInput; Pterm = error * kP; integrated += error; Iterm = kI * constrain(integrated, outMin, outMax); Dterm = (error - lastError) * kD; float calcPID = Pterm + Iterm - Dterm; if(calcPID > 0) calcPID = calcPID + 70; if(calcPID < 0) calcPID = calcPID - 70; calcPID = constrain(calcPID, outMin, outMax); // remember variables lastError = error; // send pid to drive motors return calcPID;}