PID Randomly Turns at the End

Programming Support VEXcode Pro V5 Text Tech Support
1

I’m programming my PID, and it mostly works. It will successfully drive with slew rate until it hits its target, but for some reason, it randomly turns at the end and cannot move onto the next command. I’m using rotational sensors, and I checked their calibration and I saw that no matter how many times I reset their positions, it’s never 0 (one of them is 1 and the other is 0.5, that’s just an example of what it looks like). Is this a hardware problem or a program problem?

#include "vex.h"
#include "math.h"

using namespace vex;

competition Competition;

///////////////////////////////////////////////////////////////////////////////////
//PREAUTON CODE
///////////////////////////////////////////////////////////////////////////////////
void pre_auton(void) {
  vexcodeInit();
  DrivetrainInertial.calibrate();
  
  //inertial calibration
  while(DrivetrainInertial.isCalibrating()) {
    wait(2500, msec);
  }
  //rotational calibration
  trackingLeft.resetPosition();
}
///////////////////////////////////////////////////////////////////////////////////

float velCap = 0;  //velCap limits the change in velocity and must be global
float targetLeft;
float targetRight;

void drive(float left, float right) {
  targetLeft = targetLeft + left;
  targetRight = targetRight + right;
  velCap = 0; //reset velocity cap for slew rate
  trackingLeft.resetPosition(); //reset base encoders
  trackingRight.resetPosition();

  float errorLeft;
  float errorRight;
  float kp = 2;
  float kpTurn = 0.5;
  float acc = 2;
  float voltageLeft = 0;
  float voltageRight = 0;
  float signLeft;
  float signRight;
  wait(20, msec);
  while(Competition.isAutonomous()){
    errorLeft = targetLeft - fabs(trackingLeft.position(turns)); //error is target minus actual value
    errorRight = targetRight - fabs(trackingRight.position(turns));

    signLeft = errorLeft / fabs(errorLeft); // + or - 1
    signRight = errorRight / fabs(errorRight);

    if(signLeft == signRight){
      voltageLeft = errorLeft * kp; //intended voltage is error times constant
      voltageRight = errorRight * kp;
    }
    else {
      voltageLeft = errorLeft * kpTurn; //same logic with different turn value
      voltageRight = errorRight * kpTurn;
    }

    velCap = velCap + acc;  //slew rate
    if(velCap > 12){
      velCap = 12; //velCap cannot exceed 115
    }

    if(fabs(voltageLeft) > velCap){ //limit the voltage
      voltageLeft = velCap * signLeft;
    }

    if(fabs(voltageRight) > velCap){ //ditto
      voltageRight = velCap * signRight;
    }

    if(errorRight < 0){
      LeftDrive.stop();
      RightDrive.stop();
    } else {
      LeftDrive.spin(fwd, voltageLeft, voltageUnits::volt); //set the motors to the intended speed
      RightDrive.spin(fwd, voltageRight, voltageUnits::volt);
    }
  
    wait(20, msec);
  }
}
///////////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////////
//AUTONOMOUS CODE
///////////////////////////////////////////////////////////////////////////////////
void autonomous(void) {

drive(8, 8,);
}
///////////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////////
//USER CONTROL CODE
///////////////////////////////////////////////////////////////////////////////////
void usercontrol(void) {
  while (1) {


    wait(20, msec);
  }
}

//
// main will set up the competition functions and callbacks
//
int main() {
  // set up callbacks for autonomous and driver control periods
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // run the pre-autonomous function
  pre_auton();

  // prevent main from exiting with an infinite loop
  while (true) {
    wait(100, msec);
  }
}
///////////////////////////////////////////////////////////////////////////////////
2

Not seeing anything obviously wrong, but drop that extra comma.
The if statement to stop might average both errors and test versus a small (0.1?) number rather than 0.

3

The drive function will not exit until the competition switch tells it auton is over. You might want to add the 2 error values together and compare to a near-zero threshold rather than zero exactly. Then, after you stop the motors, you can return from the function.

1 Like