File content as of revision 47:fb3c7929d3f3:

/*
    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 revA by Troy
        - Modified code to log every 1 second in current iteration
    Modified 2017-12-20 revB by Troy
        - Fixed bug where Dive depth was resetting to rise depth
    Modified 2017-12-21 revA by Troy
        - 2 minute timeout default
        - Added system time to MBED logger and OpenLog
        - Fixed bug where it was recording random keyboard presses (other FSM states)
        - Added the ability to save the batt and BCE PID config files
        - Fixed bug with FIND_NEUTRAL sub-FSM (command was not updating with each timer cycle)
*/
 
#include "mbed.h"
#include "StaticDefs.hpp"
 
// loop rate used to determine how fast events trigger in the while loop
Ticker loop_rate_ticker;
Ticker log_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)
volatile bool log_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-21 revA\n\n\r");
    
    //setup data logger baud rate and write the start of the program (every time you reset)
    datalogger().baud(57600);
    datalogger().printf("SYSTEM, RESET\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 %6.1f mm, slope %0.5f  \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 %6.1f mm, slope %0.5f  \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
    log_loop_rate_ticker.attach(&log_loop_trigger, 1.0); // fires the ticker at 1 Hz rate (every second)

    //create the MBED log file (current log file)
    //mbedLogger().openFile();
    
    //set time of logger (to current or close-to-current time)
    mbedLogger().setLogTime();
    sdLogger().setLogTime();
    
    //create log files if not present on file system
    mbedLogger().initializeLogFile();
    
    pc().printf("Size of ConfigFile: %d\n\r", sizeof(ConfigFile));
    pc().printf("Size of ConfigFileIO: %d\n\r", sizeof(ConfigFileIO));
    pc().printf("Size of IMU: %d\n\r", sizeof(IMU));
    pc().printf("Size of LinearActuator: %d\n\r", sizeof(LinearActuator));
    pc().printf("Size of LTC1298 (SpiADC): %d\n\r", sizeof(SpiADC));
    pc().printf("Size of MbedLogger: %d\n\r", sizeof(MbedLogger));
    pc().printf("Size of omegaPX209: %d\n\r", sizeof(omegaPX209));
    pc().printf("Size of OuterLoop: %d\n\r", sizeof(OuterLoop));
    pc().printf("Size of PIDController: %d\n\r", sizeof(PIDController));
    pc().printf("Size of PololuHBridge: %d\n\r", sizeof(PololuHBridge));        //fix this class
    pc().printf("Size of PosVelFilter: %d\n\r", sizeof(PosVelFilter));
    pc().printf("Size of SequenceController: %d\n\r", sizeof(SequenceController));
    pc().printf("Size of ServoDriver: %d\n\r", sizeof(ServoDriver));
    pc().printf("Size of StateMachine: %d\n\r", sizeof(StateMachine));
} 
 
int main() {
    setup();
    
    while(1) {
        static int current_state = 0;
        
        static bool file_opened = false;
        
        // FSM loop runs at 10 hz
        if(loop) {
            led1() = !led1(); // blink led 1
            current_state = stateMachine().runStateMachine();       //running State Machine. Returns 0 if sitting idle or keyboard press (SIT_IDLE state).
            loop = false; // wait until the loop rate timer fires again
        }
        
        // log loop runs at 1 hz
        if (log_loop) {
            //when the state machine is not in SIT_IDLE state (or a random keyboard press)
            if (current_state == TRANSMIT_LOG or current_state == RECEIVE_SEQUENCE) {
                //main_loop_rate_ticker.detach();
                //log_loop_rate_ticker.detach();
                
                ;   //pass
            }
            
            else if(current_state != 0) {
                if (!file_opened) {                                 //if the log file is not open, open it
                    mbedLogger().appendLogFile(current_state, 0);   //open MBED file once
                    //sdLogger().appendLogFile(current_state, 0);     //open SD file once
                               
                    file_opened = true;                             //stops it from continuing to open it

                    pc().printf(">>>>>>>> Recording. Log file opened. <<<<<<<<\n\r");
                }
                
                //record to Mbed file system     
                mbedLogger().appendLogFile(current_state, 1);    //writing data
                //sdLogger().appendLogFile(current_state, 1);    //writing data
            }
            
            //when the current FSM state is zero, reset the file
            else {
                //this can only happen once
                if (file_opened) {
                    mbedLogger().appendLogFile(current_state, 0);    //close log file
                    //sdLogger().appendLogFile(current_state, 0);    //close log file
                    
                    file_opened = false;
                    
                    pc().printf(">>>>>>>> Stopped recording. Log file closed. <<<<<<<<\n\r");
                }
            }
            
            log_loop = false;   // wait until the loop rate timer fires again
        }   //END OF LOG LOOP
    }
}