You could also go with an object-oriented approach. This thread -Very Simple Very Accurate Chassis Control Code Release - #10 by theol0403 - talks a lot about that specifically and is a more generic setup so that you can control just about anything with PID in different ways, including during the driver control period.
If you could figure out a way to include slew rate into this, that might be the best, albeit most difficult way to control a tray accurately.