PID loop problem - robot drives straight infinitely

Programming Support VEXcode V5 Tech Support
#1

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;
    vex::task::sleep(20);

    
    }
  
  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) {

  vex::task something(drivePID);
  reset = true;
  desiredvalue = 10;

  vex::task::sleep(1000);
#2

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
#3

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
#4

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
#5

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