File content as of revision 28:16c83a2fdefa:

/*
    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)
*/
 
#include "mbed.h"
#include "StaticDefs.hpp"
 
// loop rate timer for slowing down how fast while(1) runs in main()
Ticker loop_rate_ticker;
volatile bool loop = false;
void loop_trigger() { loop = true; }; // loop trigger
 
void setup() {
    pc().baud(57600);
    pc().printf("\n\n\rFSG POOL TEST 2017-12-01 revA\n\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().init();
    depth().tare();
    
    // construct a local file system
    local();
 
    // load config data from files
    configFileIO().load_BCE_config();      // load the buoyancy engine parameters from the file "bce.txt"
    configFileIO().load_BATT_config();     // load the battery mass mover parameters from the file "batt.txt"
    configFileIO().load_DEPTH_config();    // load the depth control loop parameters from the file "depth.txt" (contains neutral position)
    configFileIO().load_PITCH_config();    // load the depth control loop parameters from the file "pitch.txt" (contains neutral position)
 
    // 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());
 
    // show that the PID gains are loading from the file
    pc().printf("bce    P:%6.2f, I:%6.2f, D:%6.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:%6.2f, I:%6.2f, D:%6.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:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
    pc().printf("pitch  P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
    pc().printf("\n\r");
         
    //load sequence from file
    sequenceController().loadSequence();
 
    // establish the main loop rate
    loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate
} 
 
int main() {
    setup();
    
    while(1) {
        // does this stuff at the loop rate
        if(loop) {
            led1() = !led1(); // blink
            stateMachine().runStateMachine();
            loop = false; // wait until the loop rate timer fires again
        }
    }
}