[AustinSchuh]

We used a simple PD loop on the wrist and elevator with some gain scheduling, since it worked well enough after hand tuning. The elevator changes mass when it starts to lift the second stage, so there is a second set of constants for that case. For both the arm and elevator, they would undershoot when approaching the goal from below, and overshoot when approaching the goal from above. To solve this, we have 2 sets of constants, one per direction. This solved our overshoot problems. I also avoid integral whenever possible. We just accepted the small steady state error on the elevator and wrist, and moved on, rather than spending the extra time to tune the integral constant across 6 sets of constants. We pass a goal into the two sub systems when we hit the setpoint buttons. The manual joysticks for those two systems also tweak the goals, rather than doing anything with power.

One thing we realized when driving the robot was that it was very useful for the arm to be angled ~50 degrees above horizontal when lining up to place a tube. To facilitate this, whenever a setpoint button is hit, the elevator raises up, and then when it is all the way up, the arm tilts forwards. When the driver hits another button, the robot then goes through an "auto-score" motion which opens the claw, spits out, lowers the elevator, then raises the arm back up, and backs the robot up. This offloads a lot of stuff from the drivers.

Most of the fancy controls is in the drivetrain, which we spent easily over a month working on to tune the robot so that it was accurate and precise enough to score two tubes.