Need help correcting/tuning our PID

Hello people on the Vex Forum,

We are a first-year VRC middle school team. We qualified for the World’s and are trying to improve our auton.

Recently, we decided to switch from block code to Vex Pro to develop a working PID code to correct for the robot driving on an angle (our right side motors seems to be stronger, so the robot veers left when going forward, and veers right when coming back). We also hope to use the PID to slow the robot down as it approaches a goal (it jerks hard otherwise and pushes away the goal). Our current code in C++ is shown below. The issues we are having are:

  1. The robot drives straight forward but then drives on an angle when coming back. We tested each part separately, and they work: the robot drives forward straight; it also drives back straight (when run without the forward part). When we run the entire sequence - drive forward, spin front clamp down and raise the arm (to clamp and bring back a goal), then drive back, the robot runs straight forward, but runs on an angle when coming back (right side continues to pull).

  2. Despite running the PID code, the robot still brakes hard as it approaches the goal, so its jerks push away the goal.

We would appreciate if you can review our code and help us tune or correct our PID and/or auton code, so we can fix these two issues.

Thank you in advance for your advice!
LS

void PID(int DesiredDistance, int Velocity, bool IsForward) {
  float kP = 35;
  float kI = 60;
  float kD = 90;
  double integralLeft = 0;
  double integralRight = 0;
  int dT = 10;
  int prevErrorLeft = 0;
  int prevErrorRight = 0;
  LeftDrive.setPosition(0, turns);
  RightDrive.setPosition(0, turns);
  LeftDrive.setVelocity(Velocity, percent);
  RightDrive.setVelocity(Velocity, percent);
  while (LeftDrive.isSpinning() && RightDrive.isSpinning()){;
    double errorLeft = (DesiredDistance) - LeftDrive.position(turns)*4*3.14159265358979323846;
    double errorRight = (DesiredDistance) - RightDrive.position(turns)*4*3.14159265358979323846;
    LeftDrive.setVelocity((double (errorLeft)*float (kP))/2, percent);
    RightDrive.setVelocity((double (errorRight)*float (kP))/2, percent);
    double areaLeft = double (errorLeft) * int (dT);
    double areaRight = double (errorRight) * int (dT);
    if (integralLeft <= 120) {  
      integralLeft = double (integralLeft + errorLeft);
    }
      if (integralRight <= 120) {  
        integralRight = double (integralRight + errorRight);
    }
    if (int (errorLeft) = 0){
    integralLeft = 0;
    }
    if (int (errorRight) = 0){
      integralRight = 0;
    }
    LeftDrive.setVelocity(((double (errorLeft)*float (kP))+ double (integralLeft)*float(kI))/2, percent);
    RightDrive.setVelocity(((double (errorRight)*float (kP))+ double (integralRight)*float(kI))/2, percent);
    double deltaLeft = double (errorLeft) - double(prevErrorLeft);
    double deltaRight = double(errorRight) - double(prevErrorRight);
    double prevErrorLeft = errorLeft;
    double prevErrorRight = errorRight;
    LeftDrive.setVelocity(((double (errorLeft)*float (kP))+ double (integralLeft)*float(kI)+ double(deltaLeft)*float(kD))/2, percent);
    RightDrive.setVelocity(((double (errorRight)*float (kP))+ double (integralRight)*float(kI)+ double(deltaRight)*float(kD))/2, percent);
    wait(dT,msec);
    
  }}

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*  --------------------------------------------------------------------------*/

void pre_auton(void) {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*---------------------------------------------------------------------------*/

void autonomous(void) {
  LeftDrive.setVelocity(100, percent);
  RightDrive.setVelocity(100, percent);  
  LeftDrive.spinFor(forward, (15/3.1415926), turns, false);
  RightDrive.spinFor(forward, (15/3.1415926), turns, false);
  PID(60, 100, true);
  Clamp.spinFor(reverse, 90, degrees);
  Arm.spinFor(forward, 210, degrees, false);
  LeftDrive.spinFor(reverse, (15/3.1415926), turns, false);
  RightDrive.spinFor(reverse, (15/3.1415926), turns, false);
  PID(60, 100, true);
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void usercontrol(void) {
  // User control code here, inside the loop
  while (1) {
    BasicHold();
    LeftDrive.spin(forward);
    RightDrive.spin(forward);
    float (Axs3)= float(Controller1.Axis3.position());
    float (Axs2) = float(Controller1.Axis2.position());
    LeftDrive.setVelocity(float (Axs3), percent);
    RightDrive.setVelocity(float (Axs2), percent);

  }
    wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.
  }


//
// Main will set up the competition functions and callbacks.
//
int main() {
  // Set up callbacks for autonomous and driver control periods.
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();

  // Prevent main from exiting with an infinite loop.
  while (true) {
    wait(100, msec);
  }
}