Hey everyone! I’ve recently been working on tuning my lateral PID and the robot moves to its inputted distanced, but then just spins forever. Do you know what the issue is and how to fix it? I also wanted to note that there are no current sensors on the robot. Thank you!
float dgs = 180/(3.25 * M_PI);
//Tuning Constants for Distance
double kP = 1;
double kI = 0;
double kD = 0;
//Tuning Constants for Turn
double turnkP = 2.0;
double turnkI = 0;
double turnkD = 0;
//Establish Variables Used in Control Loop
double desiredValue = 0;
int desiredTurnValue = 0;
int error;
int prevError = 0;
int derivative;
int totalError = 0;
int turnError;
int turnPrevError = 0;
int turnDerivative;
int turnTotalError = 0;
// Allows You to Reset Your Loop
bool resetDriveSensors = false;
// Allows You to Turn Off PID Control in Driver Control
bool enableDrivePID = true;
// Drive pid
int drivePID() {
while (enableDrivePID) {
if (resetDriveSensors) {
resetDriveSensors = false;
LFrontBottom.setPosition(0, degrees);
LFrontTop.setPosition(0, degrees);
LMiddle.setPosition(0, degrees);
RFrontBottom.setPosition(0, degrees);
RFrontTop.setPosition(0, degrees);
RMiddle.setPosition(0, degrees);
//Inertial.setHeading(0,degrees);
}
int LFrontBottomPos = LFrontBottom.position(degrees);
int LFrontTopPos = LFrontTop.position(degrees);
int LMiddlePos = LMiddle.position(degrees);
int RFrontBottomPos = RFrontBottom.position(degrees);
int RFrontTopPos = RFrontTop.position(degrees);
int RMiddlePos = RMiddle.position(degrees);
////////////////////////////////////////////////////
// Drive PID
////////////////////////////////////////////////////
int averagePosition = (LFrontBottomPos + LFrontTopPos + LMiddlePos + RFrontBottomPos + RFrontTopPos + RMiddlePos)/6;
//PROPRTION
error = desiredValue - averagePosition;
//DERIVATIVE
derivative = error - prevError;
//INTEGRAL
totalError = totalError + error;
double motorPower = (error * kP + derivative * kD + totalError * kI);
///////////////////////////////////////////////////
// Turn PID
///////////////////////////////////////////////////
int turnDifference = (LFrontBottomPos + LMiddlePos + LFrontTopPos) - (RFrontBottomPos + RMiddlePos + RFrontTopPos);
//TURN PROPORTION
turnError = desiredTurnValue - turnDifference;
// turnError = DesiredTurnValue - Inertial.heading(degrees);
//TURN DERIVATIVE
turnDerivative = turnError - turnPrevError;
//TURN INTEGRAL
turnTotalError = turnTotalError + turnError;
double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI);
///////////////////////////////////////////////////
// Motor Commands
///////////////////////////////////////////////////
double powerScaler = 1.0;
double leftMotorPower = powerScaler * (motorPower - turnMotorPower);
double rightMotorPower = powerScaler * (motorPower + turnMotorPower);
LFrontBottom.spin(fwd, leftMotorPower, volt);
LMiddle.spin(fwd, leftMotorPower, volt);
LFrontTop.spin(fwd, leftMotorPower, volt);
RFrontTop.spin(fwd, rightMotorPower, volt);
RMiddle.spin(fwd, rightMotorPower, volt);
RFrontBottom.spin(fwd, rightMotorPower, volt);
prevError = error;
turnPrevError = turnError;
task::sleep(20);
}
return 1;
}
void LateralPID(int NumInches){
resetDriveSensors = true;
desiredValue = (dgs * NumInches);
}
void auton_left() {
task PID(drivePID);
LateralPID(-10);
vex::task::sleep(100);
desiredTurnValue = 90;
}