PID Autonomous Not Working

So I coded a PID, and when i try to run the PID in autonomous, nothing is moving. I will put the code bellow. Anything will help.

    double kP = 0.0;
    double kI = 0.0;
    double kD = 0.0;
    //Turn PID Contstants 
    double turnkP = 0.0;
    double turnkI = 0.0;
    double turnkD = 0.0;

    int maxTurnIntegral = 300; // These cap the integrals
    int maxIntegral = 300;
    int integralBound = 3; //If error is outside the bounds, then apply the integral. This is a buffer with +-integralBound degrees

    //Autonomous Settings
    int desiredValue = 0; // distance that needs to be traveled
    int desiredTurnValue = 0;   // how much you want it to tuen 

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

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

    bool resetDriveSensors = false;

    //Variables modified for use
    bool enableDrivePID = true;

    //Pasted from a C++ resource
    double signnum_c(double x) {
      if (x > 0.0) return 1.0;
      if (x < 0.0) return -1.0;
      return x;
    }

    int drivePID(){   /// start of function 
      
      while(enableDrivePID){

        if (resetDriveSensors) {
          resetDriveSensors = false;
          LeftMotor1.setPosition(0,degrees);
          RightMotor1.setPosition(0,degrees);
          LeftMotor2.setPosition(0,degrees);
          RightMotor2.setPosition(0,degrees);
        }


        //Get the position of both motors
        int leftMotorPosition = LeftMotor1.position(degrees);
        int rightMotorPosition = RightMotor1.position(degrees);
        int leftMotor2Position = LeftMotor2.position(degrees);
        int rightMotor2Position = RightMotor2.position(degrees);


        ////////////////////////////////////////////////////////////////////
        //Lateral movement PID
        /////////////////////////////////////////////////////////////////////
        //Get average of the two motors
        int averagePosition = ((leftMotorPosition +leftMotor2Position)  + (rightMotorPosition + rightMotor2Position))/4;

        //Potential
        error = averagePosition - desiredValue;

        //Derivative
        derivative = error - prevError;

        //Integral
        if(abs(error) < integralBound){
        totalError+=error; 
        }  else {
        totalError = 0; 
        }
        //totalError += error;

        //This would cap the integral
        totalError = abs(totalError) > maxIntegral ? signnum_c(totalError) * maxIntegral : totalError;

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


        ///////////////////////////////////////////
        //Turning movement PID
        /////////////////////////////////////////////////////////////////////
        //Get average of the two motors
        int turnDifference =(leftMotorPosition+ leftMotor2Position) - (rightMotorPosition + rightMotor2Position);

        //Potential
        turnError = turnDifference - desiredTurnValue;

        //Derivative
        turnDerivative = turnError - turnPrevError;

        //Integral
        if(abs(error) < integralBound){
        turnTotalError+=turnError; 
        }  else {
        turnTotalError = 0; 
        }
        //turnTotalError += turnError;

        //This would cap the integral
        turnTotalError = abs(turnTotalError) > maxIntegral ? signnum_c(turnTotalError) * maxIntegral : turnTotalError;

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


    /// resulting motor spin from PID

        LeftMotor1.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
        RightMotor1.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
        LeftMotor2.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
        RightMotor2.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);


        

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

      }

      return 1;
    }
---------
    void autonomous(void) {
      vex::task forward(drivePID);


     
      resetDriveSensors = true; 
      desiredValue = 900;      // move 300 degrees forward
      desiredTurnValue = 15;
      
      ///////////////////////////////////////////////////////////
      vex::task::sleep(1000);
      resetDriveSensors = true;
      desiredValue = 0;
      desiredTurnValue = 150; // turn 150 degrees
}

Did you tune your KP,KI,KD values or are they 0?

No they havent been tuned yet, I just set them at zero because it hasent been tuned yet. Does the Kp,Ki and Kd have to have values for it to do any movement?

Yes at leas Kp needs to have a value. I suggest watching the tutorial again or looking at other pid tutorials so that you can fully grasp the concept of a pid controller before you start copy and pasting.

7 Likes

yes otherwise you are multiplying by 0

Did your autonomous work after tuning the kP, kI, and kD values?

I can’t believe I’m doing this. Please do not reply to/revive old threads. @DRow could you lock this thread when you get back to the office, please?

2 Likes