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: ConfigFileIO/ConfigFileIO.cpp
- Revision:
- 21:38c8544db6f4
- Parent:
- 17:7c16b5671d0e
- Child:
- 38:83d06c294807
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ConfigFileIO/ConfigFileIO.cpp Wed Nov 22 23:04:11 2017 +0000 @@ -0,0 +1,241 @@ +#include "ConfigFileIO.hpp" +#include "StaticDefs.hpp" + +//configuration file io, both to read, set, and save + +ConfigFileIO::ConfigFileIO() { +} + +void ConfigFileIO::savePitchData(float p_gain, float i_gain, float d_gain, int batt_zeroOffset) { + ConfigFile write_pitch_txt; //write to the config file + + char PGain_buffer[128]; + sprintf(PGain_buffer,"# pitch outer loop parameters\n\n#Gains\nPGain"); + + char string_pgain[128]; + sprintf(string_pgain, "%f", p_gain); + write_pitch_txt.setValue(PGain_buffer, string_pgain); + + char string_igain[128]; + sprintf(string_igain, "%f", i_gain); + write_pitch_txt.setValue("IGain", string_igain); + + char string_dgain[128]; + sprintf(string_dgain, "%f", d_gain); + write_pitch_txt.setValue("DGain", string_dgain); + + write_pitch_txt.setValue("\n#Pitch sensor filter parameters\nfilterWn", "6.0"); + write_pitch_txt.setValue("deadband", "0.5"); + + char string_zeroOffset[128]; + sprintf(string_zeroOffset, "%d", batt_zeroOffset); + //bce setting was 41 mm during LASR experiments + write_pitch_txt.setValue("\n#Offset for neutral (default: 41)\nzeroOffset", string_zeroOffset); + + //SAVE THE DATA! + pc().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!"); + + if (!write_pitch_txt.write("/local/pitch.txt")) { + pc().printf("\n\rERROR: (SAVE)Failure to write depth.txt file."); + } + else { + pc().printf("\n\rFile pitch.txt successful written.\n\r"); + } +} + +void ConfigFileIO::saveDepthData(float p_gain, float i_gain, float d_gain, int bce_zeroOffset) { + ConfigFile write_depth_txt; //write to the config file + + char PGain_buffer[128]; + sprintf(PGain_buffer,"# depth outer loop parameters\n\n#Gains\nPGain"); + + char string_pgain[128]; + sprintf(string_pgain, "%f", p_gain); + write_depth_txt.setValue(PGain_buffer, string_pgain); + + char string_igain[128]; + sprintf(string_igain, "%f", i_gain); + write_depth_txt.setValue("IGain", string_igain); + + char string_dgain[128]; + sprintf(string_dgain, "%f", d_gain); + write_depth_txt.setValue("DGain", string_dgain); + + write_depth_txt.setValue("\n#Depth sensor filter parameters\nfilterWn", "6.0"); + write_depth_txt.setValue("deadband", "0.5"); + + char string_zeroOffset[128]; + sprintf(string_zeroOffset, "%d", bce_zeroOffset); + //bce setting was 240 mm during LASR experiments + write_depth_txt.setValue("\n#Offset for neutral (default: 240)\nzeroOffset", string_zeroOffset); + + //SAVE THE DATA! + pc().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!"); + + if (!write_depth_txt.write("/local/depth.txt")) { + pc().printf("\n\rERROR: (SAVE)Failure to write depth.txt file."); + } + else { + pc().printf("\n\rFile depth.txt successful written.\n\r"); + } +} + +int ConfigFileIO::load_BCE_config() { + ConfigFile cfg; + int count = 0; + if (!cfg.read("/local/bce.txt")) { + error("File Read Error"); + } + char value[BUFSIZ]; + + if (cfg.getValue("PGain", &value[0] , sizeof(value))) { + bce().setControllerP(atof(value)); + count++; + } + if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { + bce().setControllerI(atof(value)); + count++; + } + if (cfg.getValue("DGain", &value[0] , sizeof(value))) { + bce().setControllerD(atof(value)); + count++; + } + if (cfg.getValue("zeroCounts", &value[0],sizeof(value))) { + bce().setZeroCounts(atoi(value)); + count++; + } + if (cfg.getValue("PistonTravelLimit", &value[0], sizeof(value))) { + bce().setTravelLimit(atof(value)); + count++; + } + if (cfg.getValue("slope", &value[0], sizeof(value))) { + bce().setPotSlope(atof(value)); + count++; + } + if (cfg.getValue("filterWn", &value[0], sizeof(value))) { + bce().setFilterFrequency(atof(value)); + count++; + } + if (cfg.getValue("deadband", &value[0], sizeof(value))) { + bce().setDeadband(atof(value)); + count++; + } + + return count; +} + +int ConfigFileIO::load_BATT_config() { + ConfigFile cfg; + int count = 0; + if (!cfg.read("/local/batt.txt")) { + error("File Read Error"); + } + char value[BUFSIZ]; + + + if (cfg.getValue("PGain", &value[0] , sizeof(value))) { + batt().setControllerP(atof(value)); + count++; + } + if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { + batt().setControllerI(atof(value)); + count++; + } + if (cfg.getValue("DGain", &value[0] , sizeof(value))) { + batt().setControllerD(atof(value)); + count++; + } + if (cfg.getValue("zeroCounts", &value[0],sizeof(value))) { + batt().setZeroCounts(atoi(value)); + count++; + } + if (cfg.getValue("PistonTravelLimit", &value[0], sizeof(value))) { + batt().setTravelLimit(atof(value)); + count++; + } + if (cfg.getValue("slope", &value[0], sizeof(value))) { + batt().setPotSlope(atof(value)); + count++; + } + if (cfg.getValue("filterWn", &value[0], sizeof(value))) { + batt().setFilterFrequency(atof(value)); + count++; + } + if (cfg.getValue("deadband", &value[0], sizeof(value))) { + batt().setDeadband(atof(value)); + count++; + } + + return count; +} + +int ConfigFileIO::load_DEPTH_config() { + ConfigFile cfg; + int count = 0; + if (!cfg.read("/local/depth.txt")) { + error("File Read Error"); + } + char value[BUFSIZ]; + + if (cfg.getValue("PGain", &value[0] , sizeof(value))) { + depthLoop().setControllerP(atof(value)); + count++; + } + if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { + depthLoop().setControllerI(atof(value)); + count++; + } + if (cfg.getValue("DGain", &value[0] , sizeof(value))) { + depthLoop().setControllerD(atof(value)); + count++; + } + if (cfg.getValue("filterWn", &value[0], sizeof(value))) { + depthLoop().setFilterFrequency(atof(value)); + count++; + } + if (cfg.getValue("deadband", &value[0], sizeof(value))) { + depthLoop().setDeadband(atof(value)); + count++; + } + if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) { + depthLoop().setOutputOffset(atof(value)); + count++; + } + return count; +} + +int ConfigFileIO::load_PITCH_config() { + ConfigFile cfg; + int count = 0; + if (!cfg.read("/local/pitch.txt")){ + error("File Read Error"); + } + char value[BUFSIZ]; + + if (cfg.getValue("PGain", &value[0] , sizeof(value))) { + pitchLoop().setControllerP(atof(value)); + count++; + } + if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { + pitchLoop().setControllerI(atof(value)); + count++; + } + if (cfg.getValue("DGain", &value[0] , sizeof(value))) { + pitchLoop().setControllerD(atof(value)); + count++; + } + if (cfg.getValue("filterWn", &value[0], sizeof(value))) { + pitchLoop().setFilterFrequency(atof(value)); + count++; + } + if (cfg.getValue("deadband", &value[0], sizeof(value))) { + pitchLoop().setDeadband(atof(value)); + count++; + } + + if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) { + pitchLoop().setOutputOffset(atof(value)); + count++; + } + return count; +} \ No newline at end of file