File content as of revision 16:3363b9f14913:

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

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

void setup() {
    pc().printf("\n\n\r\rFSG 2017-11-06\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");
}

int main() {
    setup();
    
    while(1) {
        led1() = !led1(); // blink
        
        stateMachine().runStateMachine();
    }
}