# Straight Drive PID Problem VEXcode Pro V5

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

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

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?

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

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

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

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 2 Likes