File content as of revision 79:3688c3a0d7f4:

    Starting from Trent's Linear Actuator code from 2017-10-19, these modifications
    by Dan add an outer loop controller for depth and pitch to command the inner
    linear actuator loops.
    Modified 2017-10-20 revA by Dan
        - added outer loop controller, but it is hanging the mbed. (turns out it was the imu update)
    Modified 2017-10-22 revA by Dan
        - outer loop now works with a call to outerloop.update() in main loop(), not with an attached ticker
    Modified 2017-10-22 revB by Dan
        - enabled both depth and pitch outer loop controllers
        - added ability to keyboard reset
    Modified 2017-10-22 revC by Dan
        - major update to the IMU library processing to make a state machine that doesn't hang
        - added lat/lon/alt and GNSS fix information to the IMU library
        - brought out the pin names into the constructors of IMU, omega, SpiADC
    Modified 2017-10-22 revD by Dan
        - everything seems to be working, shy of re-checking on the hardware
        - Depth sensor call done inside the OuterLoop, but should somehow be set as a callback instead
        - IMU sensor call done inside the OuterLoop, but should somehow be set as a callback instead
    Modified 2017-10-23 revA by Dan/Matt
        - linear actuator hardware works great, limit switches, sensing, etc.
        - outer loops run, but move the BCE in the wrong direction.
        - new IMU code doesn't read from the sensor correctly, but doesn't hang up either.
        - depth sensor worked fine, just needs zero bias adjustment.
    Modified 2017-10-24 by Troy
        - added offset to outerloop
    Modified 2017-10-26 by Dan
        - brought over a state machine and new keyboard controls ... currently just dumped into main.
    Modified 2017-10-26 by Matt
        - new IMU code imported and working well with the hardware.
    Modified 2017-10-26 revB by Dan
        - This version has been in the pool.
        - Get occasional ADC bad string pot batt & piston. On initial sensor reads, see negative positions.
            But after running FLOAT_BROADCAST, string pot positions are normal. Not sure why.
        - Repeatedly got stuck in RISE with a +60s timeout.  Battery hit end bell and stalled out.
        - keyboard and state machine are in main, probably shouldn't be, but easier to debug.
        - Really happy with the logic and flow of the state machine. Timeouts work.
        - Need to add a means to drive the linear actuators manually ... toggle out stop() in SIT_IDLE.
        - Need to add keyboard commands to modify the zeroOffset positions.
    Modified 2017-10-30 by Dan, Trent, Matt, Troy
        - changed .stop() to .pause() and many .start() to .unpause() ... fixed the negative ADC and lets
            the PVF's keep running even though the motor isn't moving.
        - changed exit conditions to use filtered depth from the outer loop.  less noisy.
    Modified 2017-10-31 by Dan, Matt
        - added oversampling and a tare function to the depth sensor.
        - updated main and keyboard to include tare in setup() and a keyboard tare option.
        - looks like the piston isn't zeroing correctly, unclear why, should be unrelated to depth sensor.
    Modified 2017-10-31 (again) by Dan, Matt, and Troy
        - POOL TESTED!
        - Tuned the PID gains with magic.  Works to hold mostly level during BCE runs.  Dive is awesome.
        - Neutral won't do what we want ... without large depth P, it won't get to depth.
        - Dive with just P doesn't overshoot depth at 0.0 deg, but does with -20 deg.  Timeout should probably be RISE.
        - Files depth/pitch updated with tuned defaults.
    Modified 2017-11-06 by Troy
        - Added acronyms to print statements (when running this with XBee you don't know what mode the 
            hardware is in because of signal dropping out)
        - Fixed print error with setpoints by printing out the setpoint variable, not the outerloop getCommand
            (command is sent every time hardware dives, finds neutral, etc.)
        - Set the pitchCommand input to 0 on the "find neutral" command, it was sending a non-zero pitch command
        - Created a class for the StateMachine
    Modified 2017-11-14 by Troy
        - Changed tare to void function (functions does not return anything when called)
        - Added a "subclass" in the cases for the Neutral Finding Sequence (Sinking, Slowly Rise, Check Pitch (and save positions))
    Modified 2017-11-20 by Troy
        - Modified StateMachine class to separate keyboard inputs from FSM
        - Added Neutral Finding sub-statemachine
        - Verified both state machines are working with hardware on desktop
        - Added class to save neutral battery and buoyancy engine positions to neutral.cfg file
    Modified 2017-11-21 by Troy
        - Need to check remove ConfigFileIO and place saving functions into config_functions.cpp (rename ConfigFunctions.cpp)
        - Added multi-dive (and multi-rise) states to FSM (may incorporate single dive states into states)
        - Need to double-check behavior of sub-FSM but bench-top testing showed correct behavior for transition from sinking, to slowly rise, to check pitch
        - Need to add a check for the multi-dive sequence file (sequence.cfg) being loaded on the MBED
    Modified 2017-11-21 by Dan
        - removed blocker and set up a loop rate timer that runs the state machine and keyboard at 10 Hz.
        - work inside StateMachine and particularly in the findNeutralSubState.
    Modified 2017-11-22 by Troy
        - Placed config_functions into ConfigFileIO and added the ability to write the neutral positions to the depth & pitch files
        - Streamlined sub-FSM and Find_Neutral state
        - PID depth and pitch gains have been made into class variables so that they can be resaved to the depth/pitch files
        - Added the ability to save depth & pitch gains and neutral offsets (configfile writes entire file at once)
        - Minor formatting fixes
        - Fixed issue with keyboard function, the keyboard function was continuously active instead of checking pc readable in sit idle
        - Bench tested neutral finding sequence and multi-dive sequence, so far so good
        - Question: Why do we want the keyboard and FSM to run together, specifically allowing the FSM call the keyboard?
    Modified 2017-11-22 by Dan
        - added ability for substate NEUTRAL_CHECK_PITCH to move the battery toward level, then saves offsets.
        - added new entry state NEUTRAL_FIRST_PITCH that also moves the battery toward level, but doesn't save.
    Modified 2017-11-27 by Troy
        - Fixed exit condition (restart at NEUTRAL_FIRST_PITCH)
        - Pool tested, needed to run timer for longer than 5 minutes (neutral finding sequence timed out)
        - IMU fell off tape
    Modified 2017-11-28 Rev A by Dan/Troy
        - Modified NEUTRAL_FIRST_PITCH to save the battery offset into the Pitch Outer Loop before beginning neutral finding sequence before sinking
        - Pitch Outer Loop now runs NEUTRAL_SINKING and NEUTRAL_SLOWLY_RISE states (instead of batt fixed position control)
        - NEUTRAL_SINKING has been changed to a 5-second timer (vice 10 seconds)
        - NEUTRAL_SLOWLY_RISE has been changed to move the piston 2 mm each interval (vice 1 mm)
    Modified 2017-11-28 Rev B by Dan/Troy
        - Changed pitch rate margin from absolute value < 0.5 deg/sec to 5.0 deg/s
        - Changed sink timer to move piston 5 mm at a time instead of 10 mm
        - Find level motion will now run every 10 seconds instead of 5 seconds
        - Created an integer array to do a quick check of what states Find Neutral sub-FSM ran through (press "c" to see states)
        - FLOAT_BROADCAST now uses limits of battery and BCE from LinearActuator.getPosition
    Modified 2017-11-28 Rev C by Dan/Troy
        - State FIND_NEUTRAL sends pitch offset from file as first commanded
        - NEUTRAL_FIRST_PITCH and NEUTRAL_CHECK_PITCH are now moving the battery mass with getSetPosition_mm command instead of the getPosition command 
            to allow the sub-FSM time to adequately move the battery mass
        - Added "else if (substate == NEUTRAL_CHECK_PITCH)" to the NEUTRAL_FIRST_PITCH/NEUTRAL_CHECK_PITCH(NFP/NCP) state to confirm that NEUTRAL_CHECK_PITCH is working
        - Added "else" to the NFP/NCP state as an error catching mechanism
        - Modified NFP/NCP state to use pitchLoop().getPosition() instead of imu().getPitch() to use the filtered readings
        - Added several "isSubStateTimerRunning = false;" statements to the sub-FSM exits to make sure the one-shot actions work in the next state!  Definitely a fix.
        - Reworked the exiting sub-FSM to be a little clearer.
        - Moved the sub-state logger into all the sub-state transitions ... this should be done in FIND_NEUTRAL on sub-state changes instead (less copy/paste).
    Modified 2017-11-29 revA by Troy
        - changelog.txt carries previous changes
        - Incorporated email changes and did quick bench test
            1) In FIND_NEUTRAL, add bce().setPosition_mm(bceFloatPosition) to the one-shot actions after the unpause().  
            2) NEUTRAL_FIRST_PITCH, changed batt position by half from 1.0 to 0.5.
            3) NEUTRAL_SINKING and NEUTRAL_SLOWLY_RISE are using getSetPosition now
            4) previousPosition_mm deleted (and print statements changed)
            5) NEUTRAL_SLOWLY_RISE check is using bce().getSetPosition_mm()
            6) Timers replaced with _neutral_timer variable
            7) Default timeout is 8 minutes = 480 seconds
        - Redid the sub-state recording method to only update on state changes
    Modified 2017-11-29 revB by Troy
        - minor print fixes
    Modified 2017-11-29 revC by Troy
        - removed depthTolerance variable that was not being used.
    Modified 2017-11-30 revA by Troy/Dan
        - Modified state RISE to setCommand to -1.0 feet (force out of water) and go to next state (float level) when above 0.5 feet
        - Modified sub-state NEUTRAL_SINKING to only move 2.5 mm per cycle instead of 5 mm
        - Commented out home function (accidentally hit home and got stuck while at LASR)
        - Added max depth neutral and dive variables
        - Print max recorded depths and current neutral buoyancy positions
    Modified 2017-11-30 revB by Troy/Dan
        - Dive and Multi-Dive cycles now exit to FLOAT_BROADCAST state
        - FLOAT_LEVEL pitchTolerance variable increased from 1.0 degree to 5.0 degrees (PV was difficult to level in LASR pool)
        - Added the ability to record the states of the FSM and print to user using keyboard "Z"
    Modified 2017-12-01 revA by Troy
        - Minor update to 2017-11-30 revB
        - Now printing current neutral battery and BCE setpoints and max depth in neutral and dive cycle with "C" (and current sensor data)
        - Pool-tested with successful dives.           
            1) Dive working correctly.  (And max dive tracker shows that it is reaching the correct commanded depth.)
            2) Float broadcast working correctly.  (And we are now going directly from dive and rise to float broadcast.)
            3) Neutral sequence has PV diving with -10 to -20 degrees of pitch, battery command seems to be slow or inactive.
            3) Float broadcast working correctly.
            4) FSM tracker and sub-FSM tracker working.
            5) Antenna working until tail just under water (last depth recorded varies between 1.0 and 1.5 feet)
    Modified 2017-12-06 revA by Troy
        - Added "_isTimeoutRunning = false;" to the FIND_NEUTRAL successful exit case ("else if (runNeutralStateMachine() == NEUTRAL_EXIT)"
            1) This will reset the timeout when the PV has successfully found and saved the neutral BCE and battery positions
        - Successful dive and multi-dive sequences exit to FLOAT_BROADCAST state (fix was implemented in code that did not respond, reimplemented here)
            1) Also made the change to the SequenceController (which loads a file that has multiple sequences)
        - FIND_NEUTRAL starts in NEUTRAL_SINKING with fix on sub-FSM to start with NEUTRAL_SINKING (verified via mbed on bench)
    Modified 2017-12-07 revA by Troy
        - Using new neutral finding sequence, dive slowly, rise slowly, then check pitch (pitch is not active at all until final state)
        - Added additional print information (showing neutral parameters with "C")
    Modified 2017-12-07 revB by Troy
        - Momentum offsets reintroduced:
            1) CASE DIVE uses "else if (depthLoop().getPosition() > depthLoop().getCommand() - 0.5) { // including offset for low momentum approaches"
            2) CASE RISE uses "else if (depthLoop().getPosition() < depthLoop().getCommand() + 0.5) { // including offset for low momentum approaches"
        - Momentum on rise command does not exit properly, removed
    Modified 2017-12-11 revA by Troy
        - Added a logger function the main while loop
        - Added vectors (essentially resizable arrays) to hold the data in the State Machine because of memory limitations
    Modified 2017-12-12 revA by Troy
        - Logger directly implemented in StateMachine (records every 5 seconds)
    Modified 2017-12-12 revB by Troy
        - Logger fixes implemented in StateMachine, was not logging each state, fixed the reset
        - Made logger into a function call
    Modified 2017-12-13 revA by Troy
        - Added function to print log file to screen (tested on bench with multiple dives and timers)
    Modified 2017-12-18 revA by Troy
        - Redo of the code to stop opening and closing files each time
        - This should allow you to write to the open file, then close it once you exit the sequence back to SIT_IDLE
        - Start record in dive, end when you exit float broadcast...
    Modified 2018-07-23
        - Tested out multiple data transmission methods, most consistent was requesting data one packet at a time
        - Added more descriptive data to terminal output
    Modified 2018-08-02
        - Timing issue where servo jumps from 1710 to 1940, not clear why this deadband exist, need to do the math on this
    Modified 2018-09-19
        - Modified the ConfigFileIO to load and save BCE and BMM values from the file (not just load the hardcoded limits of 320 BCE and 60 BMM)
*/