Robot won't move when I start PID loop

When I start my code my robot won’t move. This is my first time ever doing PID and I followed a tutorial on YouTube.

This is the code I have made.

//Drive Settings
const int wheelTravel = 320;
const int trackWidth = 320;
const int wheelBase = 130;
motor_group driveL(leftMotorF, leftMotorM, leftMotorB);
motor_group driveR(rightMotorF, rightMotorM, rightMotorB);
drivetrain myDrivetrain(driveL, driveR, wheelTravel, trackWidth, wheelBase, mm);

//Settings
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;

//Auton Settings
int desiredVaule = 500;
int desierdTurnValue = 0;

int error;
int prevError = 0;
int derivative;
int totalError = 0;

int turnError;
int turnPrevError = 0;
int turnDerivative;
int turnTotalError = 0;

bool resetDriveSensors = false;

bool enableDrivePID = true;

int drivePID(){

while(enableDrivePID){

if(resetDriveSensors){
  resetDriveSensors = false;
  leftMotorF.setPosition(0, degrees);
  leftMotorM.setPosition(0, degrees);
  leftMotorB.setPosition(0, degrees);
  rightMotorF.setPosition(0, degrees);
  rightMotorM.setPosition(0, degrees);
  rightMotorB.setPosition(0, degrees);
}

//Get position of motors

int driveLPositionF = leftMotorF.position(degrees);
int driveLPositionM = leftMotorM.position(degrees);
int driveLPOsistionB = leftMotorB.position(degrees);
int driveRPosistionF = rightMotorF.position(degrees); 
int driveRPosistionM = rightMotorM.position(degrees);
int driveRPositionB = rightMotorB.position(degrees);

///////////////////////////////////////////////////////////////////////////////////////////////////////////
////// Lateral Movement PID
///////////////////////////////////////////////////////////////////////////////////////
//get average of the motors
int averagePosition = (driveLPositionF + driveLPositionM + driveLPOsistionB + driveRPosistionF + driveRPosistionM + driveRPositionB)/6;

//Postential
error = desiredVaule - averagePosition;

//Derivative 
derivative = error - prevError;

//Integral
totalError += error;

double lateralMotorPower = (error * kP + derivative * kD + totalError * kI)/12;
///////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////////
///////Turning movement PID
////////////////////////////////////////////////////////////////////////////////
//Get average of the two motors
int turnDifference = (driveLPositionF + driveLPositionM + driveLPOsistionB) - (driveRPosistionF + driveRPosistionM + driveRPositionB);

//Potential 
turnError = turnDifference - desierdTurnValue;

//Derivative
turnDerivative = turnError - turnPrevError;

//Intergral
turnTotalError += turnError;

double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI)/12;
//////////////////////////////////////////////////////////////////////

driveL.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
driveR.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);


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

}
return 1;
}

void autonomous(void) {

vex::task pidMovment(drivePID);
desiredVaule = 500;

vex::task::sleep(1000);

}

All of your PID constants are set to 0. You need to at least set kP to a non-zero value in order for your robot to move.

10 Likes