File content as of revision 17:7c16b5671d0e:

#include "ConfigFileIO.hpp"
#include "StaticDefs.hpp"

// This set of functions is all about the configuration file io, both to read, set, and save

// todo: update write PID because there are now more gains
// todo: back-check this file matches the list of parameters

//Files that are present on the MBED are cleared and rewritten

/* ************************** SETUP FILE SYSTEM ************************** */

// constructor
ConfigFileIO::ConfigFileIO() {
    //LocalFileSystem local("local"); //not working in the class
}

void ConfigFileIO::saveNeutralPositions(float float_nbp_value, float float_nb_battpos_value) {   
    ConfigFile write_neutral_cfg;   //write to the config file
    
    char nbp_buffer[50];  
    sprintf(nbp_buffer, "%f", float_nbp_value);
    
    if (!write_neutral_cfg.setValue("neutral_buoyancy_engine_position_mm", nbp_buffer)) {
        pc().printf("\nFailure to set a value!");
    }
    else {
        pc().printf("NET BUOYANCY (buoyancy engine position) CONFIG written!. \n\r");
    }
    
    char nb_battpos_value[50];  
    sprintf(nb_battpos_value, "%f", float_nb_battpos_value);
    
    if (!write_neutral_cfg.setValue("neutral_buoyancy_battery_motor_position_mm", nb_battpos_value)) {
        pc().printf("\nFailure to set a value!");
    }
    else {
        pc().printf("NET BUOYANCY (batt motor position) CONFIG written!. \n\r");
    }
    
    //SAVE THE DATA!
    pc().printf("Saving Neutral Buoyancy Positions!");
    
    if (!write_neutral_cfg.write("/local/neutral.cfg")) {
        pc().printf("\n\rERROR: (SAVE)Failure to write neutral.cfg file.");
    }
    else {
        pc().printf("\n\r(SAVE) Successful save.");
    } 
}

void ConfigFileIO::loadNeutralPositions() {     //reads neutral.cfg
    ConfigFile read_neutral_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
        
    char * string_neutral_buoyancy_engine_position_mm = "neutral_buoyancy_engine_position_mm";
    char * string_neutral_buoyancy_battery_motor_position_mm = "neutral_buoyancy_battery_motor_position_mm";
    char * startup_delay_config = "startup_delay_neutral";  //not using this 10/11/2017
    char * string_config_neutral_depth_ft = "neutral_dive_depth_ft";
    
    char value[BUFSIZ]; //something like size 4096    

    float config_file_wait_setting_neutral;     //you can set the delay in the config file!
    
    //read configuration file stored on MBED
    if (!read_neutral_cfg.read("/local/neutral.cfg")) {
        pc().printf("\n\rERROR:Failure to read neutral.cfg file.");
    }
    else
        pc().printf("Successfully read neutral.cfg file.\n\r");
    
    //Read values from config file
    if (read_neutral_cfg.getValue(string_neutral_buoyancy_engine_position_mm, &value[0], sizeof(value))) {
        pc().printf("\n\r%s = %s", string_neutral_buoyancy_engine_position_mm, value);
        ////xbee().printf("\n\r%s = %s", string_neutral_buoyancy_engine_position_mm, value);
        _neutral_bce_pos_mm = atof(value);  //global for now
    }
    if (read_neutral_cfg.getValue(string_neutral_buoyancy_battery_motor_position_mm, &value[0], sizeof(value))) {
        pc().printf("\n\r%s = %s", string_neutral_buoyancy_battery_motor_position_mm, value);
        ////xbee().printf("\n\r%s = %s", string_neutral_buoyancy_battery_motor_position_mm, value);
        _neutral_batt_pos_mm = atof(value);  //global for now
    }
}

float ConfigFileIO::getBattPos() {
    return _neutral_batt_pos_mm;
}

float ConfigFileIO::getBCEPos() {
    return _neutral_bce_pos_mm;
}

//// load data on command
//void ConfigFileIO::load() {
//    load_manual_position_PID_gains_from_config_file();
//    load_auto_PID_gains_from_config_file();
//    load_timers_from_config_file();
//    
//    //add rest of loading functions
//    //load_initial_config_position
//}
//
//void ConfigFileIO::write_to_config_neutral_buoyancy_position_mm(float float_nbp_value, float float_nb_battpos_value) {   
//    ConfigFile write_neutral_cfg;   //write to the config file
//    
//    char nbp_buffer[50];  
//    sprintf(nbp_buffer, "%f", float_nbp_value);
//    
//    if (!write_neutral_cfg.setValue("neutral_buoyancy_engine_position_mm", nbp_buffer)) {
//        pc().printf("\nFailure to set a value!");
//        ////////xbee().printf("\nFailure to set a value!");
//    }
//    else {
//        pc().printf("\n*********** NET BUOYANCY (buoyancy engine position) CONFIG written!. *****************");
//        ////xbee().printf("\n*********** NET BUOYANCY (buoyancy engine position) CONFIG written!. *****************");
//    }
//    
//    char nb_battpos_value[50];  
//    sprintf(nb_battpos_value, "%f", float_nb_battpos_value);
//    
//    if (!write_neutral_cfg.setValue("neutral_buoyancy_battery_motor_position_mm", nb_battpos_value)) {
//        pc().printf("\nFailure to set a value!");
//        ////xbee().printf("\nFailure to set a value!");
//    }
//    else {
//        pc().printf("\n*********** NET BUOYANCY (batt motor position) CONFIG written!. *****************");
//        ////xbee().printf("\n*********** NET BUOYANCY (batt motor position) CONFIG written!. *****************");
//    }
//    
//    //SAVE THE DATA!
//    
//    pc().printf("$$$$$$$$$$$$$$ SAVING THE DATA NOW! $$$$$$$$$$$$$$");
//    //xbee().printf("$$$$$$$$$$$$$$ SAVING THE DATA NOW! $$$$$$$$$$$$$$");
//    
//    if (!write_neutral_cfg.write("/local/neutral.cfg")) {
//        pc().printf("\n\rERROR: (SAVE)Failure to write neutral.cfg file.");
//        ////xbee().printf("\n\rERROR: (SAVE)Failure to write neutral.cfg file.");
//    }
//    else {
//        pc().printf("\n\r(SAVE) Successful save.");
//        ////xbee().printf("\n\r(SAVE) Successful save.");
//    } 
//}
//
//void ConfigFileIO::load_neutral_config() {     //reads neutral.cfg
//    ConfigFile read_neutral_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
//    
//    pc().printf("\n\rRead NEUTRAL Config File Function");
//    //xbee().printf("\n\rRead NEUTRAL Config File Function");
//    
//    char * string_neutral_buoyancy_engine_position_mm = "neutral_buoyancy_engine_position_mm";
//    char * string_neutral_buoyancy_battery_motor_position_mm = "neutral_buoyancy_battery_motor_position_mm";
//    char * startup_delay_config = "startup_delay_neutral";  //not using this 10/11/2017
//    char * string_config_neutral_depth_ft = "neutral_dive_depth_ft";
//    
//    char value[BUFSIZ]; //something like size 4096    
//
//    float config_file_wait_setting_neutral;     //you can set the delay in the config file!
//    
//    //read configuration file stored on MBED
//    if (!read_neutral_cfg.read("/local/neutral.cfg")) {
//        pc().printf("\n\rERROR:Failure to read neutral.cfg file.");
//        ////xbee().printf("\n\rERROR:Failure to read neutral.cfg file.");
//    }
//    
//    //READ THE VALUES FROM THE FILES
//    if (read_neutral_cfg.getValue(string_neutral_buoyancy_engine_position_mm, &value[0], sizeof(value))) {
//        pc().printf("\n\r%s = %s", string_neutral_buoyancy_engine_position_mm, value);
//        ////xbee().printf("\n\r%s = %s", string_neutral_buoyancy_engine_position_mm, value);
//        neutral_bce_position_mm = atof(value);  //global for now
//    }
//    
//    if (read_neutral_cfg.getValue(string_neutral_buoyancy_battery_motor_position_mm, &value[0], sizeof(value))) {
//        pc().printf("\n\r%s = %s", string_neutral_buoyancy_battery_motor_position_mm, value);
//        ////xbee().printf("\n\r%s = %s", string_neutral_buoyancy_battery_motor_position_mm, value);
//        neutral_batt_position_mm = atof(value);  //global for now
//    }
//    
//    if (read_neutral_cfg.getValue(string_config_neutral_depth_ft, &value[0], sizeof(value))) {
//        pc().printf("\n\r%s = %s", string_config_neutral_depth_ft, value);
//        ////xbee().printf("\n\r%s = %s", string_config_neutral_depth_ft, value);
//        neutral_depth_cmd = atof(value);  //global for now
//    }
//    
//    if (read_neutral_cfg.getValue(startup_delay_config, &value[0], sizeof(value))) {
//        pc().printf("\n\r%s = %s", startup_delay_config, value);
//        ////xbee().printf("\n\r%s = %s", startup_delay_config, value);
//        config_file_wait_setting_neutral = atof(value);  //global for now
//    }    
//    
//    wait(config_file_wait_setting_neutral);     //delay the rest of the program so user can check data
//}
//
////this depth is manually changed by the user
////write the neutral dive depth to the neutral.cfg or neutral config file
////not allowing modification of position FOR NOW 10/11/2017
//void ConfigFileIO::write_neutral_dive_depth_in_ft_to_config(float write_neutral_depth_ft) {
//    ConfigFile write_neutral_cfg;   //write to the config file
//    
//    char buffer_depth_ft[50];  
//    sprintf(buffer_depth_ft, "%f", write_neutral_depth_ft);
//    
//    /* ***************** convert existing data ***************** */
//    char buffer_buoyancy_position[50];  
//    sprintf(buffer_buoyancy_position, "%f", neutral_bce_position_mm);
//    
//    char buffer_battery_position[50];  
//    sprintf(buffer_battery_position, "%f", neutral_batt_position_mm);
//    /* ***************** convert existing data ***************** */
//        
//    //write to the file (print error if it didn't work)
//    
//    if (!write_neutral_cfg.setValue("neutral_buoyancy_engine_position_mm", buffer_buoyancy_position)) {    //writes Neutral_Buoyancy=4.0 //psi
//        pc().printf("\nFailure to set a value!");
//    }
//    
//    if (!write_neutral_cfg.setValue("neutral_buoyancy_battery_motor_position_mm", buffer_battery_position)) {    //writes Neutral_Buoyancy=4.0 //psi
//        pc().printf("\nFailure to set a value!");
//    }
//    
//    if (!write_neutral_cfg.setValue("neutral_dive_depth_ft", buffer_depth_ft)) {    //writes Neutral_Buoyancy=4.0 //psi
//        pc().printf("\nFailure to set a value!");
//    }
//    
//    //SAVE THE DATA!
//    if (!write_neutral_cfg.write("/local/neutral.cfg"))
//    {
//        pc().printf("\nERROR: (SAVE) Failure to write neutral.cfg file.");
//        ////xbee().printf("\nERROR: (SAVE) Failure to write neutral.cfg file.");
//    }
//    else
//    {
//        pc().printf("\n(SAVE) Successful save.");
//    }
//}
//
//// write_dive_cycle_config(max_dive_depth_in_feet,pitch_angle,net_buoyancy,dive_time_force);
//void ConfigFileIO::write_dive_cycle_config(float depth_feet, float pitch_deg, float buoyancy, float dive_time) {
//    ConfigFile write_divecyc_cfg;   //write to the config file
//    
//    char char_depth_feet[50];  
//    sprintf(char_depth_feet, "%f", depth_feet);
//    
//    char char_pitch_deg[50];  
//    sprintf(char_pitch_deg, "%f", pitch_deg);
//    
//    char char_buoyancy[50];  
//    sprintf(char_buoyancy, "%f", buoyancy);
//    
//    char char_dive_time[50];  
//    sprintf(char_dive_time, "%f", dive_time);
//    
//    float startup_delay = 0.1;
//    char char_startup_delay[50];  
//    sprintf(char_startup_delay, "%f", startup_delay);
//        
//    //write to the file (print error if it didn't work)
//    
//    if (!write_divecyc_cfg.setValue("dive_cycle_depth_cmd", char_depth_feet)) {    //writes Neutral_Buoyancy=4.0 //psi
//        pc().printf("\nFailure to set dive_cycle_depth_cmd value!");
//    }
//    
//    if (!write_divecyc_cfg.setValue("dive_cycle_pitch_cmd", char_pitch_deg)) {    //writes Neutral_Buoyancy=4.0 //psi
//        pc().printf("\nFailure to set dive_cycle_pitch_cmd value!");
//    }
//    
//    if (!write_divecyc_cfg.setValue("dive_cycle_net_buoyancy_cmd", char_buoyancy)) {    //writes Neutral_Buoyancy=4.0 //psi
//        pc().printf("\nFailure to set dive_cycle_net_buoyancy_cmd value!");
//    }
//    
//    if (!write_divecyc_cfg.setValue("emergency_surface_delay", char_dive_time)) {    //writes Neutral_Buoyancy=4.0 //psi
//        pc().printf("\nFailure to set emergency_surface_delay value!");
//    }
//    
//    if (!write_divecyc_cfg.setValue("startup_delay_dive", char_startup_delay)) {    //writes Neutral_Buoyancy=4.0 //psi
//        pc().printf("\nFailure to set startup_delay_dive value!");
//    }
//    
//    //SAVE THE DATA!
//    if (!write_divecyc_cfg.write("/local/neutral.cfg"))
//    {
//        pc().printf("\nERROR: (SAVE) Failure to write neutral.cfg file.");
//        ////xbee().printf("\nERROR: (SAVE) Failure to write neutral.cfg file.");
//    }
//    else
//    {
//        pc().printf("\n(SAVE) Successful save.");
//    }
//}
//
//void ConfigFileIO::load_dive_cycle_config() {    //reads divecyc.cfg
//    ConfigFile dive_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
//    
//    pc().printf("\n\rRead DIVE CYCLE Config File Function");
//    //xbee().printf("\n\rRead DIVE CYCLE Config File Function");
//    
//    char * string_max_depth_config_value = "dive_cycle_depth_cmd";
//    char * string_dive_cycle_pitch_angle_config_value = "dive_cycle_pitch_cmd";
//    char * string_dive_cycle_net_buoyancy_input_mL = "dive_cycle_net_buoyancy_cmd";    
//    char * startup_delay_config = "startup_delay_dive";
//    
//    char value1[BUFSIZ]; //something like size 4096
//    char value2[BUFSIZ]; //something like size 4096
//    char value3[BUFSIZ]; //something like size 4096
//    char value4[BUFSIZ]; //something like size 4096
//    char value5[BUFSIZ]; //something like size 4096
//    
//    float config_file_wait_setting_dive;     //you can set the delay in the config file!
//    
//    //read configuration file stored on MBED
//    if (!dive_cfg.read("/local/divecyc.cfg"))  {
//        pc().printf("\nERROR:Failure to read a configuration file.");
//        ////xbee().printf("\nERROR:Failure to read a configuration file.");
//    }
//    
//    //READ THE VALUES FROM THE FILES
//    if (dive_cfg.getValue(string_max_depth_config_value, &value1[0], sizeof(value1))) {
//        pc().printf("\n\r%s = %s", string_max_depth_config_value, value1);
//        ////xbee().printf("\n\r%s = %s", string_max_depth_config_value, value1);
//        dive_cycle_depth_cmd = atof(value1);
//    }
//    
//    if (dive_cfg.getValue(string_dive_cycle_pitch_angle_config_value, &value2[0], sizeof(value2))) {
//        pc().printf("\n\r%s = %s", string_dive_cycle_pitch_angle_config_value, value2);
//        ////xbee().printf("\n\r%s = %s", string_dive_cycle_pitch_angle_config_value, value2);
//        dive_cycle_pitch_cmd = atof(value2);
//    }
//    
//    if (dive_cfg.getValue(string_dive_cycle_net_buoyancy_input_mL, &value3[0], sizeof(value3))) {
//        pc().printf("\n\r%s = %s", string_dive_cycle_net_buoyancy_input_mL, value3);
//        ////xbee().printf("\n\r%s = %s", string_dive_cycle_net_buoyancy_input_mL, value3);
//        dive_cycle_net_buoyancy_cmd = atof(value3);
//    }
//}
//
//void ConfigFileIO::load_initial_config_position() {    //reads starting.cfg
//    ConfigFile starting_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
//    
//    pc().printf("\n\rRead Starting Config File Function");
//    //xbee().printf("\n\rRead Starting Config File Function");
//    
//    /* values that the motors go to when they start */
//    char * positionCmd_config = "bce_setPoint";
//    char * battery_positionCmd_config = "batt_setPoint";
//    /* values that the motors go to when they start */
//    
//    char * depth_setPoint_config = "depth_setPoint";    //pressure_PSI_setPoint
//    char * depth_controller_deadband_config = "depth_deadband";
//    char * DIVE_DEPTH_config = "DIVE_DEPTH";  //config file has DIVE_DEPTH=0.0
//
//    char value[BUFSIZ]; //something like size 4096    
//    
//    //read configuration file stored on MBED
//    if (!starting_cfg.read("/local/starting.cfg")) {
//        pc().printf("\n\rERROR:Failure to read starting.cfg file.");
//        ////xbee().printf("\n\rERROR:Failure to read starting.cfg file.");
//    }
//    
//    //READ THE VALUES FROM THE FILES
//    if (starting_cfg.getValue(positionCmd_config, &value[0], sizeof(value))) {
//        pc().printf("%\n\rs=%s", positionCmd_config, value);
//        ////xbee().printf("\n\r%s = %s", positionCmd_config, value);
//        bce_setPoint = atof(value);
//    }
//    
//    if (starting_cfg.getValue(battery_positionCmd_config, &value[0], sizeof(value))) {
//        pc().printf("\n\r%s = %s", battery_positionCmd_config, value);
//        ////xbee().printf("\n\r%s = %s", battery_positionCmd_config, value);
//        batt_setPoint = atof(value); //THIS IS WHERE YOU SET THE STUFF IN THE STARTUP
//    }
//    
//    if (starting_cfg.getValue(depth_setPoint_config, &value[0], sizeof(value))) {
//        pc().printf("\n\r%s = %s", depth_setPoint_config, value);
//        ////xbee().printf("\n\r%s = %s", depth_setPoint_config, value);
//        depth_setPoint = atof(value);
//    }
//    
//    if (starting_cfg.getValue(depth_controller_deadband_config, &value[0], sizeof(value))) {
//        pc().printf("\n\r%s = %s", depth_controller_deadband_config, value);
//        ////xbee().printf("\n\r%s = %s", depth_controller_deadband_config, value);
//        depth_deadband = atof(value);
//        //this is not coverted to PSI, do that later
//    }
//    
//    if (starting_cfg.getValue(DIVE_DEPTH_config, &value[0], sizeof(value))) {
//        pc().printf("\n\r%s = %s", DIVE_DEPTH_config, value);
//        ////xbee().printf("\n\r%s = %s", DIVE_DEPTH_config, value);
//        dive_cycle_depth_cmd = atof(value);  //write this from the config file
//    }
//    
//    //READ THE VALUES FROM THE FILES   
//    pc().printf("\n\n\rReading from starting.cfg");
//    pc().printf("\n\r(STARTING) Buoyancy_Engine_pos: %3.3f BATT_pos: %3.3f", bce_setPoint, batt_setPoint);
//    pc().printf("\n\r(STARTING) Pressure setPoint: %3.3f (deadband: %3.3f)(STARTING)", depth_setPoint, depth_deadband);
//    
//    //xbee().printf("\n\n\rReading from starting.cfg");
//    //xbee().printf("\n\r(STARTING) Buoyancy_Engine_pos: %3.3f BATT_pos: %3.3f", bce_setPoint, batt_setPoint);
//    //xbee().printf("\n\r(STARTING) Pressure setPoint: %3.3f (deadband: %3.3f)(STARTING)", depth_setPoint, depth_deadband);
//}
//
//void ConfigFileIO::load_auto_PID_gains_from_config_file() {
//    int gain_counter = 0;
//    
//    //PID.cfg
//    ConfigFile read_PID_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
//    
//    pc().printf("\n\n\rReading AUTO_PID.cfg File Function");
//    //xbee().printf("\n\n\rReading AUTO_PID.cfg File Function");
//    
//    char * string_BE_P_value = "depth_controller_P";
//    char * string_BE_I_value = "depth_controller_I";
//    char * string_BE_D_value = "depth_controller_D";
//    
//    char * string_BATT_P_value = "pitch_controller_P";
//    char * string_BATT_I_value = "pitch_controller_I";
//    char * string_BATT_D_value = "pitch_controller_D";
//    
//    char value[BUFSIZ]; //something like size 4096
//
//    float float_BE_P_value;
//    float float_BE_I_value;
//    float float_BE_D_value;
//    
//    float float_BATT_P_value;
//    float float_BATT_I_value;
//    float float_BATT_D_value;
//    
//    //read configuration file stored on MBED
//    if (!read_PID_cfg.read("/local/AUTO_PID.cfg")) 
//    {
//        pc().printf("\n\rERROR: Failure to read AUTO_PID.cfg file.");
//        ////xbee().printf("\n\rERROR: Failure to read AUTO_PID.cfg file.");
//        gain_counter++;
//    }
//    
//    //READ THE VALUES FROM THE FILES
//    if (read_PID_cfg.getValue(string_BE_P_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BE_P_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BE_P_value, value);
//        float_BE_P_value = atof(value);
//        gain_counter++;
//    }
//    
//    if (read_PID_cfg.getValue(string_BE_I_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BE_I_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BE_I_value, value);
//        float_BE_I_value = atof(value);
//        gain_counter++;
//    }
//    
//    if (read_PID_cfg.getValue(string_BE_D_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BE_D_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BE_D_value, value);
//        float_BE_D_value = atof(value);
//        gain_counter++;
//    }  
//    
//    if (read_PID_cfg.getValue(string_BATT_P_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BATT_P_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BATT_P_value, value);
//        float_BATT_P_value = atof(value);
//        gain_counter++;
//    }
//    
//    if (read_PID_cfg.getValue(string_BATT_I_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BATT_I_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BATT_I_value, value);
//        float_BATT_I_value = atof(value);
//        gain_counter++;
//    }
//    
//    if (read_PID_cfg.getValue(string_BATT_D_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BATT_D_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BATT_D_value, value);
//        float_BATT_D_value = atof(value);
//        gain_counter++;
//    } 
//    
//    /* set these global parameters from the config file */
//    depth_controller_P = float_BE_P_value;
//    depth_controller_I = float_BE_I_value;
//    depth_controller_D = float_BE_D_value;
//    
//    pitch_controller_P = float_BATT_P_value;
//    pitch_controller_I = float_BATT_I_value;
//    pitch_controller_D = float_BATT_D_value;
//    
//    if (gain_counter != 6) {
//        pc().printf("\b\rAUTO_PID gain settings have not been properly loaded!");
//        ////xbee().printf("\n\rAUTO_PID gain settings have not been properly loaded!");
//        wait(3);    //test against watchdog...
//    }
//    
//    else {
//        pc().printf("\n\rAUTO_PID gain settings loaded.");
//        ////xbee().printf("\n\rAUTO_PID gain settings loaded.");
//    }
//}
//
//// from "PID.cfg"
//void ConfigFileIO::load_manual_position_PID_gains_from_config_file() {
//    int gain_counter = 0;
//    
//    //PID.cfg
//    ConfigFile read_PID_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
//    
//    pc().printf("\n\n\rReading MAN_PID.cfg File Function");
//    //xbee().printf("\n\n\rReading MAN_PID.cfg File Function");
//    
//    char * string_BE_P_value = "buoyancy_engine_P";
//    char * string_BE_I_value = "buoyancy_engine_I";
//    char * string_BE_D_value = "buoyancy_engine_D";
//    
//    char * string_BATT_P_value = "battery_P";
//    char * string_BATT_I_value = "battery_I";
//    char * string_BATT_D_value = "battery_D";
//    
//    char value[BUFSIZ]; //something like size 4096
//
//    float float_BE_P_value;
//    float float_BE_I_value;
//    float float_BE_D_value;
//    
//    float float_BATT_P_value;
//    float float_BATT_I_value;
//    float float_BATT_D_value;
//    
//    //read configuration file stored on MBED
//    if (!read_PID_cfg.read("/local/MAN_PID.cfg")) 
//    {
//        pc().printf("\n\rERROR: Failure to read MAN_PID.cfg file.");
//        ////xbee().printf("\n\rERROR: Failure to read MAN_PID.cfg file.");
//        gain_counter++;
//    }
//    
//    //READ THE VALUES FROM THE FILES
//    if (read_PID_cfg.getValue(string_BE_P_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BE_P_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BE_P_value, value);
//        float_BE_P_value = atof(value);
//        gain_counter++;
//    }
//    
//    if (read_PID_cfg.getValue(string_BE_I_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BE_I_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BE_I_value, value);
//        float_BE_I_value = atof(value);
//        gain_counter++;
//    }
//    
//    if (read_PID_cfg.getValue(string_BE_D_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BE_D_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BE_D_value, value);
//        float_BE_D_value = atof(value);
//        gain_counter++;
//    }  
//    
//    if (read_PID_cfg.getValue(string_BATT_P_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BATT_P_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BATT_P_value, value);
//        float_BATT_P_value = atof(value);
//        gain_counter++;
//    }
//    
//    if (read_PID_cfg.getValue(string_BATT_I_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BATT_I_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BATT_I_value, value);
//        float_BATT_I_value = atof(value);
//        gain_counter++;
//    }
//    
//    if (read_PID_cfg.getValue(string_BATT_D_value, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", string_BATT_D_value, value);
//        ////xbee().printf("\n\r%s = %s", string_BATT_D_value, value);
//        float_BATT_D_value = atof(value);
//        gain_counter++;
//    } 
//    
//    /* set these global parameters from the config file */
//    bce_position_P = float_BE_P_value;
//    bce_position_I = float_BE_I_value;
//    bce_position_D = float_BE_D_value;
//    
//    batt_position_P = float_BATT_P_value;
//    batt_position_I = float_BATT_I_value;
//    batt_position_D = float_BATT_D_value;
//    
//    if (gain_counter != 6) {
//        pc().printf("\b\rMAN_PID gain settings have not been properly loaded!");
//        ////xbee().printf("\n\rMAN_PID gain settings have not been properly loaded!");
//        wait(3);    //test against watchdog...
//    }
//    
//    else {
//        pc().printf("\n\rMAN_PID gain settings loaded.");
//        ////xbee().printf("\n\rMAN_PID gain settings loaded.");
//    }
//}
//    
//
////write only the automated PID control gains
//void ConfigFileIO::write_manual_position_PID_values_to_config(float battP, float battI, float battD, float beP, float beI, float beD) {
//    ConfigFile write_PID_cfg;   //write to pid.cfg file
//    
//    pc().printf("\n\rCONFIG FILE IO write_manual_position_PID_values_to_config\n");
//    
//    char char_battP[50]; 
//    sprintf(char_battP, "%0.5f", battP);
//    char char_battI[50]; 
//    sprintf(char_battI, "%0.5f", battI);
//    char char_battD[50]; 
//    sprintf(char_battD, "%0.5f", battD);
//    
//    char char_beP[50]; 
//    sprintf(char_beP, "%0.5f", beP);
//    char char_beI[50]; 
//    sprintf(char_beI, "%0.5f", beI);
//    char char_beD[50]; 
//    sprintf(char_beD, "%0.5f", beD);   
//    
//    if (!write_PID_cfg.setValue("buoyancy_engine_P", char_beP))
//    {
//        pc().printf("\nFailure to set buoyancy_engine_P value!");
//    }
//    
//    if (!write_PID_cfg.setValue("buoyancy_engine_I", char_beI))
//    {
//        pc().printf("\nFailure to set buoyancy_engine_I value!");
//    }
//    
//    if (!write_PID_cfg.setValue("buoyancy_engine_D", char_beD))
//    {
//        pc().printf("\nFailure to set buoyancy_engine_D value!");
//    }
//    
//    if (!write_PID_cfg.setValue("battery_P", char_battP))
//    {
//        pc().printf("\nFailure to set battery_P value!");
//    }
//    
//    if (!write_PID_cfg.setValue("battery_I", char_battI))
//    {
//        pc().printf("\nFailure to set battery_I value!");
//    }
//    
//    if (!write_PID_cfg.setValue("battery_D", char_battD))
//    {
//        pc().printf("\nFailure to set battery_D value!");
//    }
//    
//    /* ************** SAVE TO PID.cfg ************** */
//    if (!write_PID_cfg.write("/local/MAN_PID.cfg")) {
//        pc().printf("\nERROR: (SAVE) Failure to write MAN_PID.cfg file.");
//        ////xbee().printf("\nERROR: (SAVE) Failure to write MAN_PID.cfg file.");
//    }
//    
//    else {
//        pc().printf("\nMAN_PID.cfg save successful.");
//        ////xbee().printf("\nMAN_PID.cfg save successful.");
//    }
//    /* ************** SAVE TO PID.cfg ************** */
//}
//
//void ConfigFileIO::write_auto_PID_values_to_config(float battP, float battI, float battD, float beP, float beI, float beD) {
//    ConfigFile write_PID_cfg;   //write to pid.cfg file
//    
//    pc().printf("\n\rCONFIG FILE IO write_auto_PID_values_to_config\n");
//    
//    char char_battP[50]; 
//    sprintf(char_battP, "%0.5f", battP);
//    char char_battI[50]; 
//    sprintf(char_battI, "%0.5f", battI);
//    char char_battD[50]; 
//    sprintf(char_battD, "%0.5f", battD);
//    
//    char char_beP[50]; 
//    sprintf(char_beP, "%0.5f", beP);
//    char char_beI[50]; 
//    sprintf(char_beI, "%0.5f", beI);
//    char char_beD[50]; 
//    sprintf(char_beD, "%0.5f", beD);   
//    
//    if (!write_PID_cfg.setValue("depth_controller_P", char_beP))
//    {
//        pc().printf("\nFailure to set depth_controller_P value!");
//    }
//    
//    if (!write_PID_cfg.setValue("depth_controller_I", char_beI))
//    {
//        pc().printf("\nFailure to set depth_controller_I value!");
//    }
//    
//    if (!write_PID_cfg.setValue("depth_controller_D", char_beD))
//    {
//        pc().printf("\nFailure to set depth_controller_D value!");
//    }
//    
//    if (!write_PID_cfg.setValue("pitch_controller_P", char_battP))
//    {
//        pc().printf("\nFailure to set pitch_controller_P value!");
//    }
//    
//    if (!write_PID_cfg.setValue("pitch_controller_I", char_battI))
//    {
//        pc().printf("\nFailure to set pitch_controller_I value!");
//    }
//    
//    if (!write_PID_cfg.setValue("pitch_controller_D", char_battD))
//    {
//        pc().printf("\nFailure to set pitch_controller_D value!");
//    }
//    
//    /* ************** SAVE TO PID.cfg ************** */
//    if (!write_PID_cfg.write("/local/AUTO_PID.cfg")) {
//        pc().printf("\n\rERROR: (SAVE) Failure to write AUTO_PID.cfg file.");
//        ////xbee().printf("\n\rERROR: (SAVE) Failure to write AUTO_PID.cfg file.");
//    }
//    
//    else {
//        pc().printf("\n\rAUTO_PID.cfg save successful.");
//        ////xbee().printf("\n\rAUTO_PID.cfg save successful.");
//    }
//    /* ************** SAVE TO PID.cfg ************** */
//}
//
//void ConfigFileIO::write_timers_to_config_file(float force, float neutral, float dive) {
//    //timers.cfg file
//    ConfigFile write_TIMERS_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
//    
//    char char_force[50];
//    sprintf(char_force, "%0.5f", force);        //convert the float values into char values
//    
//    char char_neutral[50];
//    sprintf(char_neutral, "%0.5f", neutral);    //convert the float values into char values
//    
//    char char_dive[50];
//    sprintf(char_dive, "%0.5f", dive);          //convert the float values into char values
//    
//    if (!write_TIMERS_cfg.setValue("timed_emergency_surface_seconds", char_force))
//    {
//        pc().printf("\n\rFailure to set timed_emergency_surface_seconds value!");
//        ////xbee().printf("\n\rFailure to set timed_emergency_surface_seconds value!");
//    }
//    
//    if (!write_TIMERS_cfg.setValue("timed_neutral_emergency_surface_seconds", char_neutral))
//    {
//        pc().printf("\n\rFailure to set timed_neutral_emergency_surface_seconds value!");
//        ////xbee().printf("\n\rFailure to set timed_neutral_emergency_surface_seconds value!");
//    }
//    
//    if (!write_TIMERS_cfg.setValue("timed_dive_emergency_surface_seconds", char_dive))
//    {
//        pc().printf("\n\rFailure to set timed_dive_emergency_surface_seconds value!");
//        ////xbee().printf("\n\rFailure to set timed_dive_emergency_surface_seconds value!");
//    }
//    
//    /* ************** SAVE TO timers.cfg ************** */
//    if (!write_TIMERS_cfg.write("/local/timers.cfg")) {
//        pc().printf("\n\rERROR: (SAVE) Failure to write timers.cfg file.");
//        ////xbee().printf("\n\rERROR: (SAVE) Failure to write timers.cfg file.");
//        wait(3);    //debugging
//    }
//    
//    else {
//        pc().printf("\n\rtimers.cfg save successful.");
//        ////xbee().printf("\n\rtimers.cfg save successful.");
//    }
//    /* ************** SAVE TO timers.cfg ************** */
//}
//
//void ConfigFileIO::load_timers_from_config_file() {
//    //timers.cfg file
//    ConfigFile read_TIMERS_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
//    
//    pc().printf("\n\n\rReading timers.cfg file");
//    //xbee().printf("\n\n\rReading timers.cfg file");
//
//    char * timed_emergency_surface_seconds_config = "timed_emergency_surface_seconds";
//    char * timed_neutral_emergency_surface_config = "timed_neutral_emergency_surface_seconds";
//    char * timed_dive_emergency_surface_config = "timed_dive_emergency_surface_seconds";
//    
//    char value[BUFSIZ]; //set a buffer value, 4096
//    
//    //read configuration file stored on MBED
//    if (!read_TIMERS_cfg.read("/local/timers.cfg")) 
//    {
//        pc().printf("\n\rERROR: Failure to read timers.cfg file.");
//        ////xbee().printf("\n\rERROR: Failure to read timers.cfg file.");
//    }
//    else {
//        pc().printf("\n\rTimers.cfg loaded!");
//        ////xbee().printf("\n\rTimers.cfg loaded!");
//    }   
//    
//    int timers_counter = 0;     
//    
//    if (read_TIMERS_cfg.getValue(timed_emergency_surface_seconds_config, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s=%s", timed_emergency_surface_seconds_config, value);
//        ////xbee().printf("\n\r%s=%s", timed_emergency_surface_seconds_config, value);
//        emergency_timeout = atof(value);  //write this from the config file
//        timers_counter++;
//    }
//    
//    if (read_TIMERS_cfg.getValue(timed_neutral_emergency_surface_config, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", timed_neutral_emergency_surface_config, value);
//        ////xbee().printf("\n\r%s = %s", timed_neutral_emergency_surface_config, value);
//        neutral_timeout = atof(value);  //write this from the config file
//        timers_counter++;
//    }
//    
//    if (read_TIMERS_cfg.getValue(timed_dive_emergency_surface_config, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", timed_dive_emergency_surface_config, value);
//        ////xbee().printf("\n\r%s = %s", timed_dive_emergency_surface_config, value);
//        dive_cycle_timeout = atof(value);  //write this from the config file
//        timers_counter++;
//    }
//    
//    if (timers_counter == 3) {
//        pc().printf("\n\rAll timers loaded successfully!");
//        ////xbee().printf("\n\rAll timers loaded successfully!");
//    }
//    else {
//        pc().printf("\n\rTIMERS NOT LOADED!");
//        ////xbee().printf("\n\rTIMERS NOT LOADED!");
//    }
//}
//
//
//void ConfigFileIO::read_config_file()
//{
//    ConfigFile read_cfg; //create this object (KEEP THIS LOCAL, do not mess with other config object)
//    
//    pc().printf("\n\rRead input.cfg File");
//    //xbee().printf("\n\rRead input.cfg File");
//    
//    //Ignore comments in a configuration file.
//    //Ignore empty lines in a configuration file.
//    //Keep spaces in side of keys and values.
//    
//    char * key1 = "depth_config";
//    char * key2 = "pitch_angle_config";
//    char * key3 = "net_buoyancy_config";
////    char * key4 = "MyKey";
//    
//    char value[BUFSIZ]; //something like size 4096
//    
//    //read configuration file stored on MBED
//    if (!read_cfg.read("/local/input.cfg")) 
//    {
//        pc().printf("\nERROR:Failure to read input.cfg configuration file.");
//        ////xbee().printf("\nERROR:Failure to read input.cfg configuration file.");
//    }
//    
//    //read the values from the file
//    if (read_cfg.getValue(key1, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", key1, value);
//        ////xbee().printf("\n\r%s = %s", key1, value);
//        depth_setPoint = atof(value);
//    }
//    else
//    {
//        ////xbee().printf("\n\rERROR: File is blank!");
//        pc().printf("\n\rERROR: File is blank!");
//    }
//    
//    //test the first key, if there is nothing present then the rest will not print either
//    if (read_cfg.getValue(key2, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", key2, value);
//        ////xbee().printf("\n\r%s = %s", key2, value);
//        pitch_setPoint = atof(value);
//    }
//    
//    if (read_cfg.getValue(key3, &value[0], sizeof(value)))
//    {
//        pc().printf("\n\r%s = %s", key3, value);
//        ////xbee().printf("\n\r%s = %s", key3, value);
//        neutral_bce_position_mm = atof(value);
//    }
//}