File content as of revision 10:085ab7328054:

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

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

extern "C" void mbed_reset();           // utilized to reset the mbed

void setup() {
    pc().printf("\n\n\r\rFSG 2017-10-22 revD\n\r");

    // 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().initialize();
    
    // 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().setPosition_mm(bce().getPosition_mm()); // start by not moving

    batt().init();
    batt().start();
    batt().setPosition_mm(batt().getPosition_mm()); // start by not moving

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

    pitchLoop().init();
    pitchLoop().setCommand(0.0);

    // 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  \n\r", 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  \n\r", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
    pc().printf("depth  P: %3.2f, I: %3.2f, D %3.2f, \n\r", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD());
    pc().printf("pitch  P: %3.2f, I: %3.2f, D %3.2f, \n\r", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD());
    pc().printf("\n\r");
}

void keyboard() {
    static float setpoint = 50.0;
    static bool isOuterLoopActive = false; // allow user to select if the outer loop is active
    char userInput;
    
    // check keyboard and make settings changes as requested
    if (pc().readable()) {
        // get the key
        userInput = pc().getc();

        // check command against desired control buttons
        if (userInput == '+' or userInput == '=') {
            setpoint += 1.0; //increment the command
        }
        else if (userInput == '-' or userInput == '_') {
            setpoint -= 1.0; //decrement the command
        }
        else if (userInput == 't' or userInput == 'T') {
            isOuterLoopActive = !isOuterLoopActive; // toggle the outer loop
            
            if (isOuterLoopActive)
                pc().printf("Outer loops are now active                                \n\r");
            else
                pc().printf("Outer loops are now inactive                                \n\r");
        }
        else if (userInput == '\r') {
            if (isOuterLoopActive) {
                pc().printf("setting outer loop: %3.0f                                \n\r", setpoint);
                
                depthLoop().setCommand(setpoint);
                bce().setPosition_mm(depthLoop().getOutput());  // connect outer to inner loop
                
                pitchLoop().setCommand(setpoint);
                batt().setPosition_mm(pitchLoop().getOutput()); // connect outer to inner loop
            }
            else {
                pc().printf("setting inner loop: %3.0f                                \n\r", setpoint);
                
                batt().setPosition_mm(setpoint);
                bce().setPosition_mm(setpoint);
            }
        }
        else if (userInput == '?') {
            pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
            mbed_reset();
        }
    }
    
    if (isOuterLoopActive)
        pc().printf("setpoint %3.1f, depth: %3.1f ft, bce: %3.1f mm       \r", setpoint, depth().getDepth(), bce().getPosition_mm());
        //pc().printf("setpoint %3.1f, pitch: %3.1f deg, batt: %3.1f mm       \r", setpoint, imu().getPitch(), batt().getPosition_mm());
    else
        pc().printf("setpoint %3.1f, bce:   %3.1f mm       \r", setpoint, bce().getPosition_mm());
        //pc().printf("setpoint %3.1f, batt:  %3.1f mm       \r", setpoint, batt().getPosition_mm());
}

void loop() {
    led1() = !led1(); // blink
    
    keyboard();
    
    // sequence controllers...
    // check whether depth has been triggered
    //    if so, move on to the next line of the script
    //    if done, surface or repeat
}

int main() {
    setup();
    while(1) {
        loop();
    }
}