# PID loop problem - robot drives straight infinitely

After the season’s over, I’ve started working on programming concepts I’ve only looked at briefly. I’ve started only with a P loop. Currently the robot drives forward infinitely unless stopped. I’ve tried changing the kP and turnkP constants to no avail. Any ideas what’s wrong here?

Help is much appreciated.

``````double kP = 1;

double turnKP = 1;

int desiredvalue;
int desiredturnvalue;

int error;
int preverror = 0;
int derivative;
int totalError;

int turnerror;
int turnpreverror = 0;
int turnderivative;
int turntotalError;

bool reset = false;

bool enableDrivePID = true;

int drivePID(){
while(enableDrivePID){

if(reset){
reset = false;
LeftMotor1.setPosition(0, degrees);
RightMotor1.setPosition(0, degrees);
}
//drive
int LeftMotorPosition = LeftMotor1.position(degrees);
int RightMotorPosition = RightMotor1.position(degrees);
int avgPosition = (LeftMotorPosition + RightMotorPosition)/2;

error = avgPosition - desiredvalue;
double motorPower = error*kP;

//turn
int turnDiff = -(LeftMotorPosition - RightMotorPosition)/2;

turnerror = turnDiff - desiredturnvalue;
double turnPower = error*turnKP;

LeftMotor1.spin(forward,motorPower+turnPower, voltageUnits::volt);
RightMotor1.spin(forward,motorPower - turnPower, voltageUnits::volt);

preverror = error;

}

return 1;
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void autonomous(void) {

reset = true;
desiredvalue = 10;

You’re not stopping your motors. To spin a certain amount, use spinFor(). At the moment, you’re telling them to spin at a certain speed forever. When your error is 0, stop your motors using LeftMotor1.stop(), and RightMotor1.stop()

1 Like

Couple of notes:

I believe `spinFor` is deprecated and will be removed in the future.

Don’t wait for error to be 0, you’ll likely never hit exactly zero. Decide on what is close enough to the target, and use that as the gating value to exit.

2 Likes

nope. rotateFor and other rotate… functions were removed from everything except VEXcode V5 Pro last year (and only because Pro has not had any updates and we would really like you all to move to the VSCode extension)

7 Likes

Appreciate the help, will fix the lack of a stop command and try it.