API suggestion: don't require initial pose for MecanumDrive constructor?

[madacker]

The Road Runner API effectively requires that MecanumDrive be created before trajectory actions are built. This can create an ordering problem for tournament code because it's often the student's trajectory building logic that figures out the robot's initial position as determined by red vs. blue and far vs. close. All of the student code I've seen gives a bogus pose to the initial MecanumDrive creation and then subsequently overwrites MecanumDrive::pose with the correct value. This awkwardness is exacerbated by the fact that it is not possible to query a trajectory action for its starting pose, so the starting pose has to be passed along as sideband information. I've also seen a number of times during developments where students accidentally provided different values for the robot's initial pose and the initial pose of the first trajectory, with the net result that the robot careens wildly at maximum velocity as soon as the trajectory is started (and yes, broken parts have resulted).

There is another problem with having to supply the robot's initial pose at MecanumDrive time: the robot may not actually be physically positioned at the correct initial pose yet. Because trajectory action creation is so slow, trajectory actions effectively have to be created during the initialization phase of the opmode. This means that the initial pose - and the effective zeroing of the localizer encoders - is done before "start" is invoked. This is a problem because at tournaments I've seen students place their robot on the field, invoke the opmode, and then push the robot to a precise starting position, all before "start" happens. The encoders register the physical movement so the first iteration of FollowTrajectoryAction run() sees that as a (large) deviation from the initial pose and engages the PID to unexpectedly (and ironically) move the robot back towards where it was before the student did the fine positioning.

As a potential solution to both these problems, could the QuickStart API be changed so that MecanumDrive() no longer takes an initial pose, but rather inherits it from the first pose in the first trajectory? Road Runner's convention would always be to assume that the robot's initial pose is supplied by the first trajectory when it is run._ I can't think of any use case where this is not true anyway, but if there is such a case the MecanumDrive::pose could still be manually overridden. Subsequently invoked trajectories during the same opmode run would always inherit the pose without resetting it. It seems like this logic could be implemented by keeping pose as null at initialization, with FollowTrajectoryAction and TurnAction's run() assigning the initial pose if it was null. That logic would also need to invoke Localizer::update() before setting the initial pose, so as to zero the localizer encoders to account for the second issue referenced above.

All of the above refers to MecanumDrive but the same applies to TankDrive as well, of course.

[j5155]

This is a problem because at tournaments I've seen students place their robot on the field, invoke the opmode, and then push the robot to a precise starting position, all before "start" happens. The encoders register the physical movement so the first iteration of FollowTrajectoryAction run() sees that as a (large) deviation from the initial pose and engages the PID to unexpectedly (and ironically) move the robot back towards where it was before the student did the fine positioning.

This part at least seems like an issue that could potentially be solved with documentation. Just saying to init after lining up might be acceptable.
But also, it is weird that the localizer is updating in init. I can't think of anything that would do that by default, unless you're manually updating it or running a trajectory.

[rbrott]

I changed the localizers in 9d2cd48 to ignore motion between construction and first update.