PID Question

Ok so for this PID code. On Auton settings does int desired value need to be 0 like int desired turn value. And in the end if I were to use a tracking wheel for no wheel slippage. Could I just switch the, for example, left motor or right motor with that? And would this essentially work, Like do you see any errors? And yes I understand I need to tune/find the correct kP, KI, Kd ,turnkP, turnkI, turnkD.

// PID system:
// Settings
//Must be tuned for your robot to move/work correctly
double kP = 0.0; // Error
double kI = 0.0; // Last, Increase Precision
double kD = 0.0; // To Fast or Too Slow

double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;

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

int error = 0; // SensorValue - DesireValue : Positional Value 
int previouserror = 0; // Position 20 miliseconds ago
int derivative = 0; // error - previouserror : Speed
int totalerror= 0; // totalerror = totalerror + error

int turnerror; // SensorValue - DesireValue : Positional Value 
int turnpreviouserror = 0; // Position 20 miliseconds ago
int turnderivative; // error - previouserror : Speed
int turntotalerror= 0; // totalerror = totalerror + error

bool resetDriveSensors = false;

// Variation modified for use
bool enableDrivePID = true;

int drivePID(){

  while(enableDrivePID){

    if (resetDriveSensors) {
      resetDriveSensors = false;
      LeftMotor.setPosition(0,degrees);
      RightMotor.setPosition(0,degrees);
    }


    // Get Position of Both Motors
    int leftMotorPosition = LeftMotor.position(degrees);
    int rightMotorPosition = RightMotor.position(degrees);

    //////////////////////////////////////////////////////////////////////////
    //Lateral Movement PID
    //////////////////////////////////////////////////////////////////////////

    // Average Of the Motors
    int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

    // Potential
    error = averagePosition - desiredValue;

    // Derivative
    derivative = error - previouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    totalerror += error;

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

    //////////////////////////////////////////////////
    //Turning movement PID
    //////////////////////////////////////////////////

    // Average Of the Motors
    int turnDifference = leftMotorPosition - rightMotorPosition;

    // Potential
    turnerror = turnDifference - desiredTurnValue;

    // Derivative
    turnderivative = turnerror - turnpreviouserror;

    // Integral : Velocity -> Position -> Absenent ( Position x Time )
    turntotalerror += turnerror;

    double turnMotorPower = (turnerror * turnkP + turnderivative * turnkD + turntotalerror * turnkI);

    ///////////////////////////////////////////////////////////////////////

    LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
    RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);



    //code
    previouserror = error;
    turnpreviouserror = turnerror;
    vex::task::sleep(20); 

  }



  return 1; 
}
/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {

  task whynot(drivePID);

  resetDriveSensors = true;
  desiredValue = 300; 
  desiredTurnValue = 600;

  vex::task::sleep(1000);
  resetDriveSensors = true;
  desiredValue = 300;
  desiredTurnValue = 600;
}

I see that you looked at my PID , you could change the LeftMotor to a tracking wheel if you really want to, this could make it slightly better.

1 Like

why r u calling p potential? lmao

1 Like
int desiredValue = 200;

It represents the target value so how far you want the robot to drive. Set it to the desired distance (in your case measured in motor degrees since that is what you use).


PID simply takes in sensor values and translates them to motor power. If you add tracking wheels in the same general orientation, then it is as simple as using those values instead (you will have to adjust the PID constants).


One error I did notice was…

LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);

You have to add turnMotorPower to one of the motors and subtract it from the other. Also make sure you apply it correctly as doing it backwards would multiply any small turn. (You can test this by setting lateralMotorPower to zero, running you program, and seeing how it responds when you try to turn it.)


Also if you want to make you PID more precises, I would recommend applying proper time units to the timed variables, I and D. Here is a great example of how to do that from another post I made…


Another slight note, probably useful for a judges interview, the P in PID stand for Proportional, not Potential.

3 Likes