PID Not Working Properly

Programming Support
#1

Hi, I followed @Connor 's tutorial and I’m stumped.

When I start the program, nothing happens, but when I start to turn the robot manually, it picks up forward speed and makes no effort to correct its self. it also completely ignores the desired value. The more I turn it away from its original position, the more speed it has.

I’ve also tested the turning part of the PID (I replaced the desiredValue with desiredTurnValue). When I started the code, it just drove forward.

The ‘desiredValue’ is so small because I was just changing the value to random numbers to see if it would make a difference and it didn’t.

Please take a look and see if you can find my mistakes.

rightDrive = right side of robots drivetrain (two motors grouped together)
leftDrive = left side of robots drivetrain (two motors grouped together)

VEXcode Pro V5:

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// rightDrive motor_group 12, 13
// leftDrive motor_group 18, 19
// Inertial inertial 15
// ---- END VEXCODE CONFIGURED DEVICES ----

#include “vex.h”

using namespace vex;

void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();

}

double kP = 0.0001;
double kI = 0;
double kD = 0;

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

int desiredValue = 200;
int desiredTurnValue = 0;

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

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

bool resetDriveSensors = false;

//variables (modifiable)
bool enableDrivePID = true;

int drivePID(){

while(enableDrivePID){

if (resetDriveSensors) {
  resetDriveSensors = false;

  leftDrive.setPosition(0, degrees);
  rightDrive.setPosition(0, degrees);
  
}

//////////////////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//Average
int averagePosition = (leftDrive.position(degrees) + rightDrive.position(degrees))/2;

//Potential
error = desiredValue - averagePosition;

//Derivative
derivative = error - prevError;

//Integral
totalError += error;

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

/////////////////////////////////////////////////////////////////////////
//Turning
/////////////////////////////////////////////////////////////////////////
//Potential
turnError = desiredTurnValue - Inertial.rotation(deg);

//Derivative
turnDerivative = turnError - turnPrevError;


//Integral
//turnTotalError += turnError;

double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
rightDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
leftDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);

rightDrive.setStopping(brake);
leftDrive.setStopping(brake);

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

}
return 1;
}

void autonomous(void) {
// …
// Insert autonomous user code here.
// …
vex::task MainFunction(drivePID);

resetDriveSensors = true;

desiredValue = 10;

resetDriveSensors = true;

vex::task::sleep(1000);
}

int main() {

}

#2

Have you tried tuning it? If you haven’t tuned it, adjust your kP, kI, and kD variables until the PID works good with your robot. The values are typically really small, and they probably won’t go above 1.

#3

I’ve tried values from 0.000001 to 1 and they didn’t change the outcome. thanks for the suggestion though.

#4

Did you put drivePID() in the main() function? You need to call the PID function in the main function for it to run.

#5

I just realized that you put it in the autonomous function. Are you sure that the autonomous function is run? You could also try increasing the distance that the robot drives. It is possible that the distance is too low for the robot to drive an easily noticable amount.

#6

read the top part of the original post. there is info about what happened there.