Pd loop autonomous

[2386programming]

Hello, my team is working on the autonomous for our robot. We have an autonomous of driving into the auto zone, and picking up a tote and driving into the auto zone, but we are trying to find a way to do different movements with the drive train in autonomous using the pd loop. We have a mecanum drive and would like to do something like pick up a tote, move back, and then strafe. If any assistance can be given in structure for the code that would be fantastic.

Thank you!

Team 2386