most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
Diff: Config_File_IO/ConfigFileIO.cpp
- Revision:
- 17:7c16b5671d0e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Config_File_IO/ConfigFileIO.cpp Tue Nov 21 22:03:26 2017 +0000 @@ -0,0 +1,897 @@ +#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); +// } +//} + +