So I coded a PID, and when i try to run the PID in autonomous, nothing is moving. I will put the code bellow. Anything will help.
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;
//Turn PID Contstants
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
int maxTurnIntegral = 300; // These cap the integrals
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 = 0; // distance that needs to be traveled
int desiredTurnValue = 0; // how much you want it to tuen
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(){ /// start of function
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftMotor1.setPosition(0,degrees);
RightMotor1.setPosition(0,degrees);
LeftMotor2.setPosition(0,degrees);
RightMotor2.setPosition(0,degrees);
}
//Get the position of both motors
int leftMotorPosition = LeftMotor1.position(degrees);
int rightMotorPosition = RightMotor1.position(degrees);
int leftMotor2Position = LeftMotor2.position(degrees);
int rightMotor2Position = RightMotor2.position(degrees);
////////////////////////////////////////////////////////////////////
//Lateral movement PID
/////////////////////////////////////////////////////////////////////
//Get average of the two motors
int averagePosition = ((leftMotorPosition +leftMotor2Position) + (rightMotorPosition + rightMotor2Position))/4;
//Potential
error = averagePosition - desiredValue;
//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
/////////////////////////////////////////////////////////////////////
//Get average of the two motors
int turnDifference =(leftMotorPosition+ leftMotor2Position) - (rightMotorPosition + rightMotor2Position);
//Potential
turnError = turnDifference - desiredTurnValue;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
if(abs(error) < integralBound){
turnTotalError+=turnError;
} else {
turnTotalError = 0;
}
//turnTotalError += turnError;
//This would cap the integral
turnTotalError = abs(turnTotalError) > maxIntegral ? signnum_c(turnTotalError) * maxIntegral : turnTotalError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
/////////////////////////////////////////////////////////////////////
/// resulting motor spin from PID
LeftMotor1.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor1.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
LeftMotor2.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor2.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
return 1;
}
---------
void autonomous(void) {
vex::task forward(drivePID);
resetDriveSensors = true;
desiredValue = 900; // move 300 degrees forward
desiredTurnValue = 15;
///////////////////////////////////////////////////////////
vex::task::sleep(1000);
resetDriveSensors = true;
desiredValue = 0;
desiredTurnValue = 150; // turn 150 degrees
}