PID loop not resetting the motors position

Programming Support
#1

Hi, we have been trying to implement a drive PID into our auto, but have ran into a problem.
The problem is when I want to reset the drive motors back to 0 with the resetDriveSensors to start a new forward movement it starts to turn to the left instead of moving straight.

The code listed below is what we have.


double kP = 0.073;
//double kI = 0.0;
double kD = 0.04;

double turnkP = 0.073;
//double turnkI = 0.0;
double turnkD = 0.04;


//Autonomous Settings
int desiredValue = 200;
int desiredTurnValue = 0;

double error = 0; //SensorValue - DesiredValue : Position
double prevError = 0; //Position 20 miliseconds ago
double derivative= 0; // error - prevError : Speed
//double totalError = 0; //totalError = totalError + error

double turnError= 0; //SensorValue - DesiredValue : Position
double turnPrevError = 0; //Position 20 miliseconds ago
double turnDerivative= 0; // error - prevError : Speed
//int turnTotalError = 0; //totalError = totalError + error

bool resetDriveSensors = false;

//Variables modified for use
bool enableDrivePID = true;


int drivePID(){


  while(enableDrivePID){
  

  if (resetDriveSensors) {

      resetDriveSensors = false;


      rightB.resetPosition();
      rightF.resetPosition();
      rightM.resetPosition();

      leftM.resetPosition();
      leftB.resetPosition();
      leftF.resetPosition();

      rightB.setPosition(0,degrees);
      rightF.setPosition(0,degrees);
      rightM.setPosition(0,degrees);

      leftM.setPosition(0,degrees);
      leftF.setPosition(0,degrees);
      leftB.setPosition(0,degrees);

    }
    

    //Get the position of both motors
    double leftfrontPosition = leftB.position(degrees);
    double leftmiddlePosition = leftM.position(degrees);
    double leftbackPosition = leftF.position(degrees);

    double rightBackMotorPosition = rightB.position(degrees);
    double rightmiddleMotorPosition = rightM.position(degrees);
    double rightfrontMotorPosition = rightF.position(degrees);

    ///////////////////////////////////////////
    //Lateral movement PID
    /////////////////////////////////////////////////////////////////////
     
    //Get average of the six motors
    double averagePosition = ( (leftfrontPosition + leftmiddlePosition + leftbackPosition + rightBackMotorPosition + rightmiddleMotorPosition + rightfrontMotorPosition)/6);
    
    //Potential
    error = desiredValue - averagePosition  ;
    
    //Derivative
    derivative = error - prevError;

    //integral
    //totalError += error;
    

    double lateralMotorPower = (error * kP + derivative * kD)/12.0; //+ totalError * kI;
    /////////////////////////////////////////////////////////////////////


    ///////////////////////////////////////////
    //Turning movement PID
    //////////////////////////////////////////
 
    
    double turnDifference = leftmiddlePosition - rightmiddleMotorPosition;


    //Potential
    turnError = desiredTurnValue - turnDifference;

    //Derivative
    turnDerivative = turnError - turnPrevError;

    //Integral
    //turnTotalError += turnError; 

    double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD)/12.0; //+ turnTotalError * turnkI;
    /////////////////////////////////////////////////////////////////////


    rightF.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    rightB.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
    rightM.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);

    leftF.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    leftB.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    leftM.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);

    prevError = error;
    turnPrevError = turnError;
    vex::task::sleep(20);

  }



  return 1;
}

when called in auto

void autonomous(void) {

   vex::task that(drivePID);
   
     resetDriveSensors = true;
    
     desiredValue = 600
     desiredTurnValue =  1300;

     vex::task::sleep(2000);

     resetDriveSensors = true;

     desiredValue =  1300;

}

Any advice and help would be great.

#2

I may be wrong, but try adding a desiredValue = 0 to the reset. It may be resetting motor values and positions, bit the desired value stays the same, leaving the loop to keep running for that desired value