Add vision system delay calibration routine

[gerth2]

A digital output is being set up to control turning the LED ring around the camera on and off.

Using this new functionality, we can perform calibration on the total processing time of the vision system, which will be needed for accurate feedback response:

• Turn ring off, wait until no target is seen.
• Turn ring on, start timer.
• Wait until vision system registers a target
• Stop timer when a target is seen, report elapsed time.

This should be implemented as a state machine with the following states:

• NO_CAL (default)
  • Do not attempt calibration
  • Leave LED ring on by default
  • Provide an interface (public method) which can be used to turn the ring on and off.
  • On the transition of the run-cal input from false to true, go to CAL_INIT
• CAL_INIT
  • Clear out all previous calibration history
  • Clear the cal failed flag.
  • Go to CAL_CYCLE_PREP
• CAL_CYCLE_PREP
  • Turn ring off
  • Wait for no target to be registered
  • If no target registered, go to CAL_START
• CAL_CYCLE_START
  • Turn ring on
  • Start Timer
  • go immediately to CAL_WAIT
• CAL_CYCLE_WAIT
  • Do nothing, but check to see if target is seen.
  • If more than 5 seconds have elapsed, assume something is wrong. Set a flag to indicate the cal has failed, and go straight to CAL_TERMINATE.
  • if a target is seen, transition to CAL_FINISH
• CAL_CYCLE_FINISH
  • report delta time (save into a java arrayList)
  • Go to CAL_TERMINATE if run command is false, otherwise go back to CAL_START
• CAL_TERMINATE
  • Using all delta times recorded during the last CAL cycle, calculate the average and std. deviation of the samples. Record these in state variables.
  • Go to NO_CAL

If the vision system goes offline during any of the CALCYCLE* states, set the cal failed flag and go straight to CAL_TERMINATE. Report that the last cal has failed, but also report out any info achieved before the failure. Note that this requires your CAL_TERMINATE logic must handle the case where the arrayList of samples is empty.

A public method must be provided for turning the LED ring on and off while in NO Cal

For integration, hook this up to a calibration on the web page so that by setting the calibration to 1, it triggers this camera calibration routine to run (until it is set back to zero).

Additionally, use the no-cal LED ring on/off method to only have the LED ring on if the vision processing system is online. This will provide an extra visual indication to the drivers of whether the vision processing system is functional or not.

If we have the circuit to control the led ring, we can use that for testing. If not, we can manually turn the ring on and off on the power supply to get the state machine through its states.

[gerth2]

Splitting CAL_CYCLE_FINISH into CAL_CYCLE_FINISH_PASS and CAL_CYCLE_FINISH_FAIL to make things cleaner.

[gerth2]

cal_failed flag is now an enum

[gerth2]

tested and merged, I'll call it done for now.