PID and code help

I made a PID, but its really inconsistent and it never works 100 percent of the time. Right now our PID is kinda broken so I’m going to have to record it, so are there any tips I should know so that it will work after? Here are also some additional questions. :pray:

Also where do I put programming and driver skills code in the file? And how do I make an auton selector other than making a separate file for each one?

Thanks.

should I do tasks or functions for pid

I prefer functions because I think it’s easier to use and is nicer code to read, but both work

how would I make a threshold of timeout for it

For a function you can make the function return when the error is close to zero, or make it return if the time since it started is > a certain amount

if (abs(error) < threshold || currentTime() > startingTime + timeout) {
    return;
}

You can also do it so if it’s in the threshold a “counter” variable is increased, and then exit the function if the counter is > a certain amount. This makes sure it stays in the threshold for x amount of time

For a PID in a thread you can do it the same, but instead of putting the conditions in the thread you put it in a waitUntil to replace the wait.

Should I do the PID calculations in degrees or inches

It doesn’t matter, but I like inches for drives and degrees for turns.

I feel like heading always as issues because turning left requires some weird stuff

turning shouldn’t need too much weird stuff, if the problem is with the robot not being able to turn past 0 degrees you can make an “angle error” function instead of subtracting to get the error. This fixes the problem where the quickest route to get to the target crosses 0 degrees, making it turn the wrong way (Ex: current rotation is 359 degrees, and target is 1 degree, error should be +2 but instead it’s -358). Here’s pseudocode of my team’s angle error function:

function angle_error(angle_a, angle_b)
{
    var wrapped_a = wrap_angle_180_180(angle_a);
    var wrapped_b = wrap_angle_180_180(angle_b);

    var error = wrapped_a - wrapped_b;

    if (abs(error) > 180) {
        error = error - sgn(error) * 360;
    }

    return error;
}

where wrap_angle_180_180 wraps the angle between -180 and 180, abs returns the absolute value of the number, and sgn returns the sign of the number (-1 for negative, 0 for 0, and 1 for positive)

And it makes the inertial drift whenever we want the PID to go laterally and turn at the same time

Nothing you can do in your code should be causing the inertial to drift (unless you are resetting the 0 position of the inertial to the wrong spot a lot) because it is a hardware thing, could you elaborate on what you mean by this?

skills is programmed the exact same as normal matches, in driver skills the driver period is ran and in auto skills the auto period is ran.

You can put “buttons” on the brain screen to make a gui using the drawing and touchscreen functions. Whenever these buttons are pressed, you set the value of a variable, and when the auto period starts you run different functions based on the value of the variable. You can probably find some people’s auto selectors to use as examples by searching on the forum.