File content as of revision 104:426224a55f5f:

    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)
    Modified FSG PCB V_1_1
        - Freezes when doing a dive or any timed sequence (commented out SD card references)
        - commented out sdLogger().appendLogFile(current_state, 0);     //open SD file once
        - commented out sdLogger().appendLogFile(current_state, 1);    //writing data
        - commented out sdLogger().appendLogFile(current_state, 0);    //close log file
        - reduced timer to 20 seconds for bench testing
        - modified ConfigFileIO for rudder()
        - added in getFloatUserInput function from newer code
        - changed LinearActuator & batt() in StaticDefs to match new pinouts from Bob/Matt/Troy fixes
        - slowed down battery motor because it's silly fast at 30 volts (bench test)
            * BCE gain is proportional 0.1 now 0.01
        - BATT was moving in the wrong direction, gain was P: 0.10, I: 0.0, D: 0.0
            * change gain to P: -0.10 (gain was flipped, I think the old circuit board had the voltages flipped ? ?)
        - StateMachine changes for testing
            * added keyboard_menu_STREAM_STATUS();
            * added keyboard_menu_RUDDER_SERVO_settings();
        - modified the zero on the battery position from 610 to 836
        - BMM (batt) slope may be incorrect, seems low, currently 0.12176
        - modified the zero on BCE from 253 to 460
        - Pressure readings are wrong
            * added readADCCounts() to omegaPX209 class to see channel readings
            * modified omegaPX209 class to use filtered ADC readings from SpiADC.readCh4()
        - fixed rudderLoop to headingLoop from newer code
    Modified FSG PCB V_1_2
        - added init headingLoop to main
        - added pitch and heading outputs to STREAM_STATUS
    NOTE: Flipped motor controller output on connector side with battery mass mover (BMM)
        - Motor direction was opposite the BCE motor (because of gearing) 
        - BMM P gain is now positive 0.02 (from -0.10)
    Modified FSG PCB V_1_3
        - added timing code for interrupt that drives the rudder (testing with o-scope)
        - PID controller replaced with newer version from 5/29 code branch
        - StateMachine hanged style of variables to match convention in code
    Modified FSG PCB V_1_4
        - adc tests
    Modified FSG PCB V_1_5
        - IMU update
        - Testing print outs
    Modified FSG PCB V_1_6
        - new BMM zero count of 240 (confirmed manually)
        - new BCE zero count of 400 (confirmed manually)
        - Modified emergency climb to go to position 10 on the BMM, not zero (almost relying on the limit switch)
        - fixed keyboard input; including typing in the timeout (can enter exact times)
    Modified FSG PCB V_1_7
        - removed data logger references
        - fixed bug where I was logging data twice in the interrupt code and the main file
        - fixed bug where Multi-Dive sequence wasn't restarting (the counter used to get the current state was not reset)
    Modified FSG PCB v_1_7C
        - fixing a bug with the data transmission (output had newline character) because of packet lengths
        - new version blasts data and Python program requests lost packets, dramatically faster over USB, slightly faster over XBee
        - fixed some format and terminal errors
    Modified code 2018-08-06
        - fixed bug with heading save feature, wasn't updating deadband properly
        - modified input on servo sub-menu
    Modified code 2018-08-07
        - Float broadcast changed to command rudder to center in that state (whatever center is in the file)    
        - Was sending the wrong command to the position dive (BMM)
    Modified code 2018-08-14
        - Fixed the deadband on "case Float_Broadcast" so it exits properly, I was seeing the BCE stop around 317 so I made a fixed deadband of 4.0 (anything 316 or above will exit the state)
        - FIND_NEUTRAL was code was double-checked:
            1) Starts with an offset of 25 mm from the manually found neutral BCE position (still buoyant), BMM starts at that neutral position
            2) case NEUTRAL_SINKING: Every 5 seconds it retracts the BCE 5 mm.  When it reaches the input depth setpoint it switches to NEUTRAL_RISING
            3) case NEUTRAL_SLOWLY_RISE:  Every 5 seconds it extends the BCE 2 mm.  When it starts to rise (depth rate is negative) (depth rate positive is sinking, negative is rising) it will immediately stop
             and go to case NEUTRAL_CHECK_PITCH.  Removed an incorrect line of code that was setting BCE position with depthloop output.
            4) case NEUTRAL_CHECK_PITCH: Every 10 seconds the pitch is adjusted +/- 0.5 mm depending on whether the nose is pitched up or pitched down
                 (the timing is because the PV may move around in the water before it stabilizes)
        - Homing Sequence now moves at -0.5 not -0.1 (was way too slow)
    Modified code at Carderock (2018-08-15)
        - Removed -1 multiplier from IMU outputs
        - Pitch is incorrectly oriented now, pitch down
        - Heading/Yaw is off by 90 degrees I believe (north is not north)
        - Did not check roll
        - KNOWN ISSUE of log file not having consistent line length (will throw off python data receiver)
    Modified code at Carderock (2018-08-16)
        - Observations at LASR lead to fixes on the save PID files, zero offsets were saving incorrectly, fixed
    Modified code at NRL DC (11/8/2018)
        - Latest code includes SD card library as a folder (not repo)
        - Comment fix on main.cpp
        - SD card working on PCB v2.4 with initial testing, commented out in this code