File content as of revision 61:f7437daae608:

/*
    Modified FSG PCB V_1_1
        - Freezes when doing a dive or any timed sequence (commented out SD card references)
        - commented out sdLogger().appendLogFile(current_state, 0);     //open SD file once
        - commented out sdLogger().appendLogFile(current_state, 1);    //writing data
        - commented out sdLogger().appendLogFile(current_state, 0);    //close log file
        - reduced timer to 20 seconds for bench testing
        - modified ConfigFileIO for rudder()
        - added in getFloatUserInput function from newer code
        - changed LinearActuator & batt() in StaticDefs to match new pinouts from Bob/Matt/Troy fixes
        - slowed down battery motor because it's silly fast at 30 volts (bench test)
            * BCE gain is proportional 0.1 now 0.01
        - BATT was moving in the wrong direction, gain was P: 0.10, I: 0.0, D: 0.0
            * change gain to P: -0.10 (gain was flipped, I think the old circuit board had the voltages flipped ? ?)
        - StateMachine changes for testing
            * added keyboard_menu_STREAM_STATUS();
            * added keyboard_menu_RUDDER_SERVO_settings();
        - modified the zero on the battery position from 610 to 836
        - BMM (batt) slope may be incorrect, seems low, currently 0.12176
        - modified the zero on BCE from 253 to 460
        - Pressure readings are wrong
            * added readADCCounts() to omegaPX209 class to see channel readings
            * modified omegaPX209 class to use filtered ADC readings from SpiADC.readCh4()
        - fixed rudderLoop to headingLoop from newer code
    Modified FSG PCB V_1_2
        - added init headingLoop to main
        - added pitch and heading outputs to STREAM_STATUS
            
    NOTE: Flipped motor controller output on connector side with battery mass mover (BMM)
        - Motor direction was opposite the BCE motor (because of gearing) 
        - BMM P gain is now positive 0.02 (from -0.10)
        
    Modified FSG PCB V_1_3
        - added timing code for interrupt that drives the rudder (testing with o-scope)
        - PID controller replaced with newer version from 5/29 code branch
        - StateMachine hanged style of variables to match convention in code
        
*/
 
#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)

/****************** for rudder servo    ******************/
Ticker systemTicker;

volatile unsigned int timer_counter = 0;

bool setup_complete = false;

static void system_timer(void) {    
    timer_counter++;
    
    //only start these updates when everything is properly setup (through setup function)
    if (setup_complete) {
            if ( (timer_counter % 20) == 0 ) {    // 0.02 second intervals
                rudder().runServo();
            }
    }
}

/****************** for rudder servo    ******************/

void setup() {
    pc().baud(57600);
    pc().printf("\n\n\rFSG PCB V1.4 2018-06-18 \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 the MBED local file system
    local();
    
    // construct the SD card file system
//    sd_card();
 
    // 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());
    
    //new 6/8/2018 for testing
    headingLoop().init();           
    headingLoop().setCommand(0.0);  //setting heading to 0 for testing
    //new 6/8/2018 for testing
 
    // 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)
 
    //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();
//    sdLogger().initializeLogFile();

    //tare the pressure sensor during setup
    depth().tare();

    setup_complete = true;  //used for interrupt timing

} 
 
int main() {
    setup();
    
    //this is used to control timing on the rudder servo, it's based on the new timing model in the 5/29/18 code with one interrupt running everything...may or may not work here
    
    systemTicker.attach_us(&system_timer, 1000);         // Interrupt timer running at 0.001 seconds       (slower than original ADC time interval)

    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
    }

}