Hello! Our team has a competition coming up soon and we are coding a PID loop for our autonomous. I followed this tutorial to get a base code down. I then modified that code for our teams 4-motor drive. I also used the inertial sensor value as the ‘target value’ for the turning portion of the PID. After tuning the constants I was able to get the Lateral movement PID to work. I am now happy with how that portion is working. However, I am struggling to get the turning movement to behave properly.
The turning PID has done several things. It has either speed up, as it turned around and got closer to the target value, or the robot would just spin at a constant speed, or it would move very slowly and eventually stop but it overshoot the target. I have been unable to determine exactly what causes each of these behaviors and it seems semi-random. However, I know the constants have something to do with it.
I try to change the turnKp constant but I have not gotten a satisfactory fix. When I decrease the constant the robot moves very slowly and eventually stops while overshooting the target or i have noticed that when I increase the constant the robot spins continuously. I have double checked my code multiple times and I have been unsuccessful in figuring out what I need to fix.
I attached my code to this post.
//Settings __________________________
double Kp = 0.22;
double Ki = 0.0;
double Kd = 0.2;
double turnKp = 0.2;
double turnKi = 0.0;
double turnKd = 0.0;
int integralBound = 3;
// auton settings
int desiredValue = 0;
int desiredDistance = 0;
int desiredTurnValue = 0;
int error; //Current sensor value - desired value : Position
int prevError = 0; // Error 20ms ago
int derivitive; //error - prevError : Speed
int totalError = 0; //total error over time
int turnError; //Current sensor value - desired value : Position
int turnPrevError = 0; // Error 20ms ago
int turnDerivitive; //error - prevError : Speed
int turnTotalError = 0; //total error over time
bool enableSetSensor = true;
bool enableDPID = true;
int drivetrainPID() {
Brain.Screen.print("PID ran");
Brain.Screen.newLine();
while(enableDPID){
while (enableSetSensor){ //re-setting all of the sensors when enableSetSensor is true
enableSetSensor = false;
Brain.Screen.print("set sensor ran");
Brain.Screen.newLine();
Inertial.setRotation(0, degrees);
RightB.setPosition(0, degrees);
RightF.setPosition(0, degrees);
LeftB.setPosition(0, degrees);
LeftF.setPosition(0, degrees);
}
desiredValue = desiredDistance / .0349; //converting from inches to degrees to get the value to input to the motor
//Brain.Screen.print(desiredValue);
//Brain.Screen.newLine();
//getting the values from the internal encoders
int RightBPos = RightB.position(degrees);
int RightFPos = RightF.position(degrees);
int LeftBPos = LeftB.position(degrees);
int LeftFPos = LeftF.position(degrees);
//Get average position of the motors
//////////////////////////////////////////////////////////////////////
//Lateral PID
/////////////////////////////////////////////////////////////////////
int averagePosition = (RightBPos + RightFPos +LeftBPos + LeftFPos)/4 ;
//Potential
error = desiredValue - averagePosition;
//Brain.Screen.print(error);
//Brain.Screen.newLine();
//derivative
derivitive = error - prevError;
//Integral
totalError += error;
/*
if (error == 0){
totalError = 0;
}
if(abs(error) < integralBound){
totalError+=error;
} else {
totalError = 0;
}
*/
double lateralmotorPower = (error*Kp + derivitive*Kd + totalError*Ki)/12;
//////////////////////////////////////////////////
//Turning PID
////////////////////////////////////////////
int inertialReading = Inertial.rotation(degrees); //setting the variable to the current position in relation to calibration
Brain.Screen.print(desiredTurnValue);
Brain.Screen.newLine();
Brain.Screen.print(turnError);
Brain.Screen.newLine();
//Potential
turnError = desiredTurnValue - inertialReading;
//derivative
turnDerivitive = turnError - turnPrevError;
//Integral
turnTotalError += turnError;
//preventing integral from becoming to high
/*
if (turnerror == 0){
turntotalError = 0;
}
if(abs(error) < integralBound){
turntotalError+=turnerror;
} else {
turntotalError = 0;
}
*/
double turnMotorPower = (turnError*turnKp + turnDerivitive*turnKd + turnTotalError*turnKi)/12;
//--------------Setting Motor Velocity -------------------//
LeftB.spin(forward, lateralmotorPower - turnMotorPower, voltageUnits::volt);
LeftF.spin(forward, lateralmotorPower - turnMotorPower, voltageUnits::volt);
RightB.spin(forward,lateralmotorPower + turnMotorPower, voltageUnits::volt);
RightF.spin(forward, lateralmotorPower + turnMotorPower, voltageUnits::volt);
//Setting the previous errors to the current errors, so the next time the code runs through the prevErrors are 20ms behind
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
return 1;
}
During the autonomous section this is what I call:
vex::task drivePIDtask(drivetrainPID);
desiredTurnValue = 360;
vex::task::sleep(1000);
A couple of notes regarding my code:
-I commented out the integral barriers, one because I don’t fully understand how they work, or what they do and I don’t feel an immediate need to use them as integral was not necessary for my drive PID portion to work properly. Therefore I am really using a PD loop.
-I was trying to use the print statements to help me debug the code, but it did not help.
Does anyone have any suggestions on how to fix this?