File content as of revision 32:f2f8ae34aadc:

/*
    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 2017-12-19 revA by Troy
        - Fixed OpenLog printing to include states and variable names.  Currently logs to LOG#####.TXT files
            1) Note: The OpenLog only starts a new log when it is power-cycled (with the MBED)
    Modified 2017-12-20 rev A by Troy
        - Modified code to log every 1 second in current iteration
*/
 
#include "mbed.h"
#include "StaticDefs.hpp"
 
// loop rate used to determine how fast events trigger in the while loop
Ticker loop_rate_ticker;

volatile bool loop = false; //used so the compiler does not optimize this variable (load from memory, do not assume state of variable)

void loop_trigger() { loop = true;} // loop trigger (used in while loop)
//void log_loop_trigger() { log_loop = true;} // log loop trigger (used in while loop)


void setup() {
    pc().baud(57600);
    pc().printf("\n\n\rFSG POOL TEST 2017-12-20 revA (DESKTOP SAVE TEST)\n\n\r");
    
    //setup data logger baud rate and write the start of the program (every time you reset)
    datalogger().baud(57600);
    datalogger().printf("DATA, LOGGER, START\n");
 
    // 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
    
    // setup the data logger rate
//    loop_log_rate_ticker.attach(&loop_log_trigger, 5.0); // fires the ticker at 0.2 Hz rate (every 5 seconds)

} 
 
int main() {
    setup();
    
    while(1) {
        // runs at 10 hz
        if(loop) {
            led1() = !led1(); // blink led 1
            stateMachine().runStateMachine();
            loop = false; // wait until the loop rate timer fires again
        }
        
//        if (log_loop) {
//            led3() = !led3(); // blink led 3
//            datalogger().printf("");
//            log_loop = false;   // wait until the loop rate timer fires again
//        }
    }
}