Seed start position and rotation in auto

[jkleiber]

Summary

When we start auto we should seed our initial pose in case vision doesn't work

The way to do this is to add an initial position and rotation field to our auto strategy files. Then have drive seed to that pose (or a correctly mirrored pose if on another alliance) on auto init

We should have red alliance be our default and have blue be the mirrored pose. Or we could have 2 separate poses of that's easier

[linglejack06]

@jkleiber how do we make sure that robot always starts at this position? like if we seed to one position but aiden places our robot 6 inches off will that seriously mess up?

[jkleiber]

@linglejack06 we could just not seed if we have received enough accepted vision measurements. The main goal here is to make it so we don't do insane driving in auto if we lose vision (so we can A-stop less frantically)

[linglejack06]

okay, so just query vision on autonomous init? and then if no pose make drive have a pose there? or should this be in disabled periodic?

[jkleiber]

@linglejack06 we should count how many valid vision measurements we get in disabled periodic and then if that's greater than some configurable constant we don't seed on auto init. Otherwise we seed

Also we should put an indication on snakescreen for this that runs in disabled (in general it would be nice to have snakescreen update some of its state in disabled periodic as well)

[linglejack06]

ok, ill add the robot code stuff soon

[minhlpnguyen]

does it matter if strategy autoinit or drive autoinit is called first?
also where can i find the starting poses? thanks!!

[aidnem]

Not sure on the auto init order. You can find strategy auto init from comp logs or using advantagescope/pathplanner.