PD Loop causes endless spinning and overall weird behavior

Programming Support
,
1

I have been testing my PD controller so that we are ready for auton. However, while testing today, the turning part of my controller has been an issue. We use an inertial sensor to calculate the turn error, which gets calibrated in pre-auto. The overall weird behavior is that it endlessly spins, the spin turns counterclockwise, and none of the tuner variables need to be changed.

double kP = 0.5;
double kD = 0.5;
double TkP = 0.5;
double TkD = 0.5;

int error = 0;
int prevError = 0;
int derivative = 0; 
double targetDist = 0.0;

int tError; 
int tPrevError = 0; 
int tDerivative = 0;
double targetTurn;

// Boolean variables: true or false variables

bool resetSens = false; // Used to rest the tracking wheel position to 0 when turned true
bool pdON = true; // Used to make sure the robot is not correcting during driver period


int drivePD() {
  while (pdON) {
    if (resetSens) {
      resetSens = false;
      // Reset track positions
      rightTrack.setPosition(0, degrees); 
      leftTrack.setPosition(0, degrees); 
    }

    // Gets the position of where the bot is in degrees
    int leftPosition: leftTrack.position(degrees);
    int rightPosition = rightTrack.position(degrees);
    
calculates the average of the tracking wheel positions
    int avg =  (leftPosition + rightPosition)/2;

calculates the error and the derivative
    error = targetDist - avg; 
    derivative = error minus prevError;

Lateral Motor Power
    double latMotorPower = error * kP + derivative * kD;

    //////////////////////////////////////////////////////////////////
    
calculates the error and the derivative for turning 
    tError = targetTurn - Inertial.rotation(degrees);
    tDerivative = tError - tPrevError;

Rotational Motor Power
    double turnMotorPower = tError * TkP + tDerivative * TkD;

    
    // Set motor speed in voltage
    leftBack.spin(forward, latMotorPower, turnMotorPower, volt);
    leftMid.spin(forward, latMotorPower, turnMotorPower, volt);
    leftFront.spin(forward, latMotorPower, turnMotorPower, volt);
    rightBack.spin(forward, latMotorPower + turnMotorPower, volt);
    rightMid.spin(forward, latMotorPower + turnMotorPower, volt);
    rightFront.spin(forward, latMotorPower + turnMotorPower, volt);
    
    // Update previous errors
    prevError = error; 
    tPrevError = tError;

    vex::task::sleep(20);
  }
    
  return 1; 
}

and this is the auto-call

void autonomous(void) {
  pdON = true;
  resetSens = true;
  vex::task PD(drivePD);
  targetTurn = 90;
}

Probably helpful information:
This is run on the VS Code extension, and the PD Controller is run in a separate file, then called into auton. I don’t think the separation really affects anything because the lateral movement works just fine.

In short, how do I get my PD controller to stop endlessly spinning?

2

It looks like you’re using the motor spin commands wrong, when you are controlling the left motors on the drivetrain you have a comma between latMotorPower and turnMotorPower, but you probably want a subtraction symbol. Also, if your robot is spinning infinitely no matter what you make your PID constants, it could be spinning the wrong way.

3

The commas were a typo and I’m pretty sure it’s spinning the wrong way because I have the left side subtract and the right side adds. Either how it doesn’t settle spinning and I don’t want to add an error limit like:

if (tError > 4){
     Motor power
}

Unless it’s necessary

4

What I @iseau395 said is definitely right but you should also turn your PID values starting with a kd of 0. I highly doubt tuning got you all 4 values to be the same 0.5.

5

Correct I I haven’t gotten the tuning variables to be 0.5. I was testing yesterday and I moved the PD function into main.cpp and then tested it; the robot didn’t spin nonstop and drove back and forth. From there I started to tune it and it was working better than Tuesday. I’m pretty sure that it was doing this because when I calibrated it in main.cpp it wouldn’t send that data to the separate function file. I’ll be able to test more on Friday.

6

As of yesterday, I have solved the problem. After printing out the inertial sensor’s value and the error term, I saw that it would keep progressing after 360 (it got to like 1489 or something). Because of this, I realized that the .rotation(degrees) method did not have a set limit. I ended up writing:

double InertPos = Inertial.rotation(degrees);

    if(InertPos < -180){
      InertPos = 180;
    } else if (InertPos > 180){
      InertPos = -180;
    }

    TurnError = targetTurn - InertPos;