Add fault detection and auto-shutoff to shooter if encoder is unplugged

[aidnem]

Purpose The purpose of this PR is to disable the shooter arm if the encoder is detected to have failed.

Detection criteria: If the arm is commanded to move at above 2x the kS, and the measured velocity is less than some value for some persistence, switch to manual arm control. At this point, the arm is just set to a permanent override of 0 volts until the code is restarted when a fault is detected, and the fault state is logged under scoring/fault.

Project Scope

• [ ] Add auto cutoff for shooter arm per above specification
• [ ] Fine tune movement threshold and amount of time before shutoff to detect faults when necessary but also to avoid false positives.

[aidnem]

Current main issue (from last night's shop): arm was being commanded to move at some voltage even when disabled (but obviously not moving). Once teleop was enabled, the arm would begin moving, but there was a large delay before the enabled code started running (indicated by the delay before the LED scheme changed), meaning that the fault detection was not running for a relatively long period of time while the arm was actively moving. This meant that, even with a cap of just 4 volts on the aimer, I still had to manually disable the arm before a fault was detected.

[aidnem]

@jkleiber I can't come to shop today because of conflicting schedules but I think once the issue with non zero voltages being sent out by the scoring subsystem to the end arm when disabled is fixed, this should be functional (except for potentially tuning it to avoid false positives). I can work on this in shop on Monday if you don't get to it today.

[jkleiber]

@aidnem everyone was focused on VHSL essay revisions today so we didn't do much. I won't be in shop on Monday, but if you get it working that would be cool.

Just build up speed slowly so you don't break the arm. We also need to figure out why the encoder isn't reading anything in the first place.

It might also be worth considering what happens if the arm drives the other way. We would probably fault in that case, but there's a chance we would fault erroneously with the encoder working in that scenario. Something to think about, and we probably don't need to worry about it rn (better to make actual monitors happen tbh)

[aidnem]

@jkleiber yeah I forgot to mention that right now all the math is done assuming a positive voltage and a positive encoder velocity. No absolute values are used, so it won't fault at all on negative voltages or velocities. If I understand correctly, we can trust the hard stop on the bottom of the arm's range of motion, right? I think that if we put fault detection for negative velocities it'll false-positive when the arm hits the hard stops at the bottom, as the arm would stop moving. I could still try to add fault detection for negatives and then create a special exception for that if needed.

Monitors: I was also thinking about how monitors could work, and it's been simmering in the back of my head for a while. Could we implement it as a subsystem, and then pass it providers for subsystem IO and functions to disable various parts of the robot when faults are detected? I'd be interested in potentially implementing that and then moving this fault detector to be the first monitor we have if you think that would be beneficial at this point.

[jkleiber]

@aidnem making monitors inherit subsystems may be a good idea. As long as they implement a common interface, are generalizable, and don't add a bunch of overhead I'd say give it a try. It'll at least be testable in sim

[aidnem]

@jkleiber I've created #199 for further discussion on monitors.