I am a beginner in the realm of PID control loops, so if anybody is willing to help me point out any issues with my program that would be incredibly appreciated.
The issue is that our robots drivetrain just wont move at all.
Code Pasted below:
//Settings
double KP = 0.4;
double KI = 0.2;
double KD = 0.4;
double turnKP = 0.1;
double turnKI = 0.01;
double turnKD = 0.1;
int maxTurnIntegral = 300; //These cap the integral
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 = 200;
int desiredturnValue = 0;
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(){
while(enabledrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
leftfront.setPosition(0,degrees);
leftback.setPosition(0,degrees);
rightfront.setPosition(0,degrees);
rightback.setPosition(0,degrees);
}
//Motor positions
int leftfrontPosition = leftfront.position(degrees);
int leftbackPosition = leftback.position(degrees);
int rightfrontPosition = rightfront.position(degrees);
int rightbackPosition = rightback.position(degrees);
////////////////////////////////////////////////////
//Lateral Movement PID
///////////////////////////////////////////////////////////////////
//Average of the motor positions
int averagePosition = (leftfrontPosition + leftbackPosition + rightfrontPosition + rightbackPosition)/4;
//Potential
Error = averagePosition - desiredturnValue;
//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
///////////////////////////////////////////////////////////////////
int turnDifference = leftfrontPosition + leftbackPosition - rightfrontPosition + rightbackPosition;
//Potential
turnerror = turnDifference - desiredturnValue;
//Derivative
turnderivative = turnerror - turnprevError;
//Integral
if(abs(turnerror) < integralBound) {
turntotalError+=turnerror;
}
else {
turntotalError = 0;
}
//turntotalError += turnerror;
//This would cap the integral
turntotalError = abs(turntotalError) > maxTurnIntegral ? signnum_c(turntotalError) * maxTurnIntegral : turntotalError;
double turnMotorPower = turnerror * turnKP + turnderivative * turnKD + turntotalError * turnKI;
/////////////////////////////////////////////////////////////////
leftfront.spin(forward, lateralMotorPower, voltageUnits::volt);
rightfront.spin(forward, lateralMotorPower, voltageUnits::volt);
leftback.spin(forward, lateralMotorPower, voltageUnits::volt);
rightback.spin(forward, lateralMotorPower, voltageUnits::volt);
/*leftfront.spinFor(forward, lateralMotorPower + turnMotorPower, inches * 28.6478897964,degrees,false);
leftback.spinFor(forward, lateralMotorPower + turnMotorPower, inches * 28.6478897964,degrees,false);
rightfront.spinFor(forward, lateralMotorPower + turnMotorPower, inches * 28.6478897964,degrees,false);
rightback.spinFor(forward, lateralMotorPower + turnMotorPower, inches * 28.6478897964,degrees);*/
//code
prevError = Error;
turnprevError = turnerror;
vex::task::sleep(20);
}
return 1;
}
edit code tags added by mods, please remember to use them.