Straight Drive PID Problem VEXcode Pro V5

Programming Support
,
1

Here is robot’s straight drive PID control loop:

double distkP = 2;
double distkI = 0.005;
double distkD = 0.1;

double diffkP = 2;
double diffkI = 0;
double diffkD = 0;
 
double driveTarget;
double distSpeed;
double diffSpeed;
 
double distError; 
double diffError;
 
double prevDistError;
double prevDiffError;
 
double diffIntegral;
double distIntegral;
 
double distDerivative; 
double diffDerivative;

bool enableDrivePID = true; 
 
void resetEncoders(){
  RightMotor.setPosition(0,degrees);
  LeftMotor.setPosition(0,degrees);
}
 
void driveStraight(int distance) // cm
{
  resetEncoders();
 
  while (distError != -1)
  {
    Brain.Screen.printAt( 10, 50, "distError %6.2f", distError);
    Brain.Screen.printAt( 10, 80, "diffError %6.2f", diffError);
    double leftEncoder = (LeftMotor.position(degrees) / 360) * 31.92;
    double rightEncoder = (RightMotor.position(degrees) / 360) * 31.92;

    distError = distance - ((leftEncoder + rightEncoder)/2); //Calculate distance error
    diffError = leftEncoder - rightEncoder; //Calculate difference error
 
    // Find the integral ONLY if within controllable range AND if the distance error is not equal to zero
    if( std::abs(distError) < 2 && distError != 0)
    {
      distIntegral = distIntegral + distError;
    }
    else
    {
      distIntegral = 0; //Otherwise, reset the integral
    }
 
    // Find the integral ONLY if within controllable range AND if the difference error is not equal to zero
    if( std::abs(diffError) < 60 && diffError != 0)
    {
      diffIntegral = diffIntegral + diffError;
    }
    else
    {
      diffIntegral = 0; //Otherwise, reset the integral
    }
 
    distDerivative = distError - prevDistError; //Calculate distance derivative
    distDerivative = diffError - prevDiffError; //Calculate difference derivative
 
    prevDistError = distError; //Update previous distance error
    prevDiffError = diffError; //Update previous difference error
 
    distSpeed = (distError * distkP) + (distIntegral * distkI) + (distDerivative * distkD); //Calculate distance speed
    diffSpeed = (diffError * diffkP) + (diffIntegral * diffkI) + (diffDerivative* diffkD); //Calculate difference (turn) speed
 
    prevDistError = 0;
    prevDiffError = 0;
 
    LeftMotor.spin(forward, distSpeed - diffSpeed, voltageUnits::volt); //Set motor values
    RightMotor.spin(forward, distSpeed + diffSpeed, voltageUnits::volt); //Set motor values
      {
    }
    if(distError < 1 && distError > -1){
      RightMotor.stop();
      LeftMotor.stop();
      break;
      }
  }
}

The problem is, the robot stops when the break statement comes in but the right motor is continuing to move like 1 sec more. And the main question is, is there any problem with my code?

1 Like
2

Could perhaps the Left motor be set to brakeType::brake and the Right motor set to brakeType::coast? That could explain why it goes through the entire loop only to have a problem after it finishes.

I would alter your stopping code to

      RightMotor.stop(brakeType::brake);
      LeftMotor.stop(brakeType::brake);

And see if it does anything.

3 Likes
3

Thank you for the answer. I realized my Omni-wheel which located rear is causing movement at the end of the loop. Because of that, I moved them to the front. Apart from these do you see any mistake in the code?

4

The only (obvious) mistake I see is this.

You set distDerivative twice, when the second should probably be diffDerivative.

Also, your exit condition is not the strongest, even though it should work. Perhaps use a timer.

4 Likes
5

Good catch @trontech569!

Also, I don’t see any delay inside the loop.

Perhaps, initialize both Integrals to zero, add vex::task::sleep(20) to the loop, and don’t reset prev*Error to zero.

4 Likes
6

I’m not really a code guy, so im gonna shoot for friction issues in your drivebase?

7

Thank you for everything, I handled the problem. The problem was like I said Omni-wheels on the back. But your PID comments made it better :slight_smile:

2 Likes