[Jared Russell]

Miss Daisy has done this for 2 years now.

It's actually not a 2 variable PID problem; it's two 1 variable PID problems.

A velocity control loop looks like this:

Velocity Command --> Velocity PID Controller--> Motor Output (with encoder counts/cycle looping back to the PID controller)

A traditional position control loop looks like this:

Position Command --> Position PID Controller --> Motor Output (with encoder counts looping back to the PID controller)

However, the problem with position command directly affecting a motor output is that (1) motors are rarely linear in response, which is what you want for an ideal PID control system, and (2) there is no guarantee that multiple motors will get to the same position in the same amound of time.

The easiest solution? Instead of generating a motor output from your position loop, generate a velocity command. The velocity controller will ensure that it reaches that speed, and it's response will be more linear, making tuning much easier. Your combined controller would look like this:

Position Command --> Position PID Controller --> Velocity Command --> Velocity PID Controller --> Motor Output (with encoder counts looping to position controller and encoder counts/cycle looping to velocity controller)

This makes user/autonomous code highly redundant: use the same velocity loop in both modes, but in user get the velocity command from the joystick position, and in autonomous get the velocity command from the position control loop (with setpoints generated by dead reckoning, sensors, etc.).

If you need help with how to code this, let me know, but this should get you started.