File content as of revision 17:7c16b5671d0e:

/*
    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
*/

#include "mbed.h"
#include "StaticDefs.hpp"
#include "config_functions.h"

volatile bool blocker = true;   //volatile used to prevent compiler from optimizing this (assuming the state is constant)
volatile bool keyboard_flag = true;

Ticker blocker_ticker;

//blocker using a ticker
void reset_blocker() {
    blocker = false;
};

void setup() {
    pc().printf("\n\n\r\rFSG 2017-11-20\n\r");
    pc().baud(57600);

    // start up the system timer
    systemTime().start();

    // set up and start the adc. This runs on a fixed interval and is interrupt driven
    adc().initialize();
    adc().start();
    
    // set up and start the imu. This polls in the background
    imu().initialize();
    imu().start();
    
    // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
    depth().init();
    depth().tare();
    
    // construct a local file system
    local();

    // load config data from files
    load_BCE_config();      // load the buoyancy engine parameters from the file "bce.txt"
    load_BATT_config();     // load the battery mass mover parameters from the file "batt.txt"
    load_DEPTH_config();    // load the depth control loop parameters from the file "depth.txt"
    load_PITCH_config();    // load the depth control loop parameters from the file "pitch.txt"

    // set up the linear actuators.  adc has to be running first.
    bce().init();
    bce().start();
    bce().pause(); // start by not moving

    batt().init();
    batt().start();
    batt().pause(); // start by not moving

    // set up the depth and pitch outer loop controllers
    depthLoop().init();
    depthLoop().start();
    depthLoop().setCommand(stateMachine().getDepthCommand());

    pitchLoop().init();
    pitchLoop().start();
    pitchLoop().setCommand(stateMachine().getPitchCommand());

    // do not leave this in. Check that PID gains are loading
    pc().printf("bce    P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f  \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
    pc().printf("batt   P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f  \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
    pc().printf("depth  P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
    pc().printf("pitch  P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
    pc().printf("\n\r");
    
    //set the neutral positions in the FSM
    // load neutral motor positions from the neutral.cfg file
    configFileIO().loadNeutralPositions();
        
    // in discussion said this was actually in pitch.txt and depth.txt
    // fixing on Wednesday (Troy)
    pc().printf("\n\rLoading neutral positions from file.\n\r");
    stateMachine().setNeutralPositions(configFileIO().getBattPos(), configFileIO().getBCEPos());     //batt, bce
    
    //load sequence from file
    sequenceController().loadSequence();
}



int main() {
    setup();
    
    blocker_ticker.attach(&reset_blocker, 0.01); // 100 Hz loop rate
    uint32_t n = 0;
    
    while(1) {
        led1() = !led1(); // blink
        
        //"blocker" while loop runs at 100 hz because of the ticker
        while(blocker) {            
            if (keyboard_flag) {
                stateMachine().keyboard();          //current concept runs keyboard and state machine in a while loop
                stateMachine().runStateMachine();
            
                keyboard_flag = false;
            }
        }
        
        //this runs at 10 hz
        if (n%10 == 0)
        {  
            keyboard_flag = true;   //the flag controls the rate at which this operates
        }   
        
        //keyboard_flag = true;   //set the keyboard flag
        blocker = true;         //reset the blocker while loop when this is completed
    }
}