[Arm] -- Init of motor encoder at reboot sometimes fails or gives bad result

[msonnier]

Expected operation

Occasionally on reboot or code restart we get inaccurate encoder value initialized into arm motor encoder on restart. This has 2 root causes:

• There is a race condition on power cycle where we try and read values from Cancoder before its ready OR before the Falcon motor is ready to accept the encoder update
• At the extreme ranges of motion on arm, there is mechanical slop that we have to account for to get a consistent match between motor encoder and Cancoder absolute encoder

Assumptions you can make

• It's OK to add a little delay on robot startup/boot.
• Slop is predictable for zones at very top and bottom extremes of arm range of motion
• Note there is a zone where arm is hanging straight down where slop is less predictable. This is NOT an area we tend to ever leave arm positioned, so hopefully we NEVER get a code reset or disable/enable in this position as we would get a bad encoder value match. Maybe we should consider some way for drivers to manually reset this during match (similar to resetting field orient on driving?)

Implementation notes

The following notes should help you navigate how to implement this Command Sequence. You can always ask your Programming Lead or Mentor for help if needed. Please explicitly call out any new EMPTY commands or functions in your pull request with this change when you submit it for review..

To try and account for mechanical slop, we look at arm absolute position every encoder reset. If it's at extreme top or bottom of range of motion, we assume gravity is "pulling arm" into the opposite end of mechanical slop. We subtract the empirically measured slop from Cancoder reading before applying that value into motor encoder in these cases.

[msonnier]

Cancoder had lost power and we still inited the encoder against a bogus value. Need to do some kind of check at init and if cancoder not present and it's autoinit we KNOW where we should be so set from that. If it's Teleop Init and we can't see Cancoder not sure what to do.