10955M PID

void SetMotorBreakingType(brakeType type = brakeType::coast) {
  backLeft.setStopping(type);
  backRight.setStopping(type);
  frontRight.setStopping(type);
  frontLeft.setStopping(type);

}

const int accel_step = 9;
const int deccel_step = 100; // no decel slew
static int leftSpeed = 0;
static int rightSpeed = 0;

void LeftMotorFBS(int leftTarget){
  int step;

  if(abs(leftSpeed) < abs(leftTarget))
    step = accel_step;
  else
    step = deccel_step;

  if(leftTarget > leftSpeed + step)
    leftSpeed += step;
  else if(leftTarget < leftSpeed - step)
    leftSpeed -= step;
  else
    leftSpeed = leftTarget;

  LeftMotorFB(leftSpeed);
}
//slew control
void RightMotorFBS(int rightTarget){
  int step;

  if(abs(rightSpeed) < abs(rightTarget))
    step = accel_step;
  else
    step = deccel_step;

  if(rightTarget > rightSpeed + step)
    rightSpeed += step;
  else if(rightTarget < rightSpeed - step)
    rightSpeed -= step;
  else
    rightSpeed = rightTarget;

  RightMotorFB(rightSpeed);
}

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);

}

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 = distIntegral + distError;
    }
    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 >= distance && timeOut >= 0);

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



void autonomous(void) {


enableDrivePID = true;
driveStraight(-100);

enableDrivePID = true;
driveStraight(100);

}