PID Function Help

Hi! My team recently started working on PID to make our autonomous more consistent, and so far it has been working great. However, we have one function where we want to run our intakes at the same time as our wheel motors, and we want our intakes to move at a constant 100% velocity. For some reason, our function works perfectly the first and third time that it is called in our autonomous code, but the second time it is called, only the wheels move and the intakes do not move at all.
Here is the function:

void getBallPID(double target){
leftForward.setPosition(0,vex::rotationUnits::rev);
wait(750, msec);

while(drivePos < target){
drivePos = leftForward.position(vex::rotationUnits::rev);
error = target - drivePos;

integral = integral + error;
if(error <= 0){
integral = 0;
}
if(error > 25){
integral = 0;
}

derivative = error - prevError;
prevError = error;

power = ((error*kP) + (integral*kI) + (derivative*kD));

wait(15, msec);

leftForward.spin(vex::directionType::fwd,power,velocityUnits::rpm);
rightForward.spin(vex::directionType::fwd,power,velocityUnits::rpm);
leftBack.spin(vex::directionType::fwd,power,velocityUnits::rpm);
rightBack.spin(vex::directionType::fwd,power,velocityUnits::rpm);

intakeMotorLeft.spin(vex::directionType::fwd,100,velocityUnits::pct);
intakeMotorRight.spin(vex::directionType::fwd,100,velocityUnits::pct);
}
leftForward.stop(vex::brakeType::coast);
leftBack.stop(vex::brakeType::coast);
rightForward.stop(vex::brakeType::coast);
rightBack.stop(vex::brakeType::coast);

intakeMotorLeft.stop(vex::brakeType::coast);
intakeMotorRight.stop(vex::brakeType::coast);
}

And here is where it is called in our autonomous route.

void skills(){
//gets first ball
getBallPID(os);
//scores the preload in the first goal
inertiaTurnPID(-125);
bumpForward(60);
scoreBall(0.8, 100);
//gets second ball
backPID(0.65);
inertiaTurnPID(0.05);
forwardPID(2.5);
getBallPID(2.3);
//scores first ball/second goal
inertiaTurnPID(-90);
bumpForward(50);
scoreBall(0.85, 100);
//gets third ball
backPID(1.4);
inertiaTurnPID(0.1);
getBallPID(os*2);

I’m not sure if the problem is something really simple that I’m missing, so help is appreciated. Thank you!

The difference between the first/third getBallPID(); and the second is the os is being used with the function and in the second it isn’t.

Is there some reason os isn’t in the second getBallPID();?.
And is there someway you can change the second getBallPID(); to include os in it?

2 Likes

I think, the problem is that all variables, including drivePos, are declared in the global scope. The first time you call getBallPID() function, drivePos value is zero and it lets you enter while loop:

However, the second time you call getBallPID(), drivePos already has a non-zero value that is larger than target=2.3, which prevents you from entering the loop.

The best way to fix this is to declare such variables local to the function and initialize them to zero before they are used.

Also, you may want to check out some of the PID drive code examples in topics linked from this post.

3 Likes

“os” is a float which is defined as the number of revolutions our wheel motors need to move to travel from one end of a field tile to the other. we have tried using “os” in the second instance as well, but we had the same issue.

1 Like

thank you so much! i’ll try this during our next meeting and let you know how it goes.

1 Like