double distkP = 0.05; ///0.05
double distkI = 0.01; ///0.01
double distkD = 0.1; //0.1

double diffkP = 0.05;
double diffkI = 0.0;
double diffkD = 0.0;

double driveTarget;
double distSpeed;
double diffSpeed;

double distError;
double diffError;

double prevDistError;
double prevDiffError;

double diffIntegral;
double distIntegral;

double distDerivative;
double diffDerivative;

bool enableDrivePID = true;

void resetEncoders(){
  frontLeft.setPosition(0,degrees);
  frontRight.setPosition(0,degrees);
  backLeft.setPosition(0,degrees);
  backRight.setPosition(0,degrees);

}

bool isDrive = true;

int sleepTimeout = 10;

void driveStraight(int distance, int timeOut = 1250) // cm
{

  resetEncoders();

  while (distError != 1)
  {
    Brain.Screen.printAt( 10, 50, "distError %6.2f", distError);
    Brain.Screen.printAt( 10, 80, "diffError %6.2f", diffError);
    double leftEncoder = (frontLeft.position(degrees) + backLeft.position(degrees)/2);

    leftEncoder = (leftEncoder/ 360)* 53.28;

    double rightEncoder = (frontRight.position(degrees) + backRight.position(degrees)/2);
    rightEncoder = (rightEncoder/ 360)* 53.28;

    distError = distance - ((leftEncoder + rightEncoder)/2); //Calculate distance error
    diffError = leftEncoder - rightEncoder; //Calculate difference error

    // Find the integral ONLY if within controllable range AND if the distance error is not equal to zero
    if( std::abs(distError) < 2 && distError != 0)
    {
      distIntegral = 0;
    }
    else
    {
      distIntegral = 0; //Otherwise, reset the integral
    }

    // Find the integral ONLY if within controllable range AND if the difference error is not equal to zero
    if( std::abs(diffError) < 60 && diffError != 0)
    {
      diffIntegral = diffIntegral + diffError;
    }
    else
    {
      diffIntegral = 0; //Otherwise, reset the integral
    }

    distDerivative = distError - prevDistError; //Calculate distance derivative
    diffDerivative = diffError - prevDiffError; //Calculate difference derivative

    prevDistError = distError; //Update previous distance error
    prevDiffError = diffError; //Update previous difference error

    distSpeed = distError * distkP + distIntegral * distkI + distDerivative * distkD; //Calculate distance speed
    LeftMotorFBS(distSpeed);
    RightMotorFBS(distSpeed);

    diffSpeed = (diffError * diffkP) + (diffIntegral * diffkI) + (diffDerivative* diffkD); //Calculate difference (turn) speed

    frontLeft.spin(fwd, distSpeed  + diffSpeed, voltageUnits::volt); //Set motor values
    frontRight.spin(fwd, distSpeed + diffSpeed, voltageUnits::volt); //Set motor values
    backLeft.spin(fwd, distSpeed - diffSpeed, voltageUnits::volt); //Set motor values
    backRight.spin(fwd, distSpeed + diffSpeed, voltageUnits::volt);

    wait(sleepTimeout, msec);
    timeOut = timeOut - sleepTimeout;
  } while (distError > -1 && timeOut >= 0) {

SetMotorBreakingType(); ////All four motors stop
enableDrivePID = false;

  }
}