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
ConfigFileIO/ConfigFileIO.cpp@43:891baf306e0a, 2018-02-14 (annotated)
- Committer:
- tnhnrl
- Date:
- Wed Feb 14 21:54:03 2018 +0000
- Revision:
- 43:891baf306e0a
- Parent:
- 38:83d06c294807
- Child:
- 52:f207567d3ea4
added new configfileIO
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tnhnrl | 17:7c16b5671d0e | 1 | #include "ConfigFileIO.hpp" |
tnhnrl | 17:7c16b5671d0e | 2 | #include "StaticDefs.hpp" |
tnhnrl | 17:7c16b5671d0e | 3 | |
tnhnrl | 21:38c8544db6f4 | 4 | //configuration file io, both to read, set, and save |
tnhnrl | 17:7c16b5671d0e | 5 | |
tnhnrl | 17:7c16b5671d0e | 6 | ConfigFileIO::ConfigFileIO() { |
tnhnrl | 17:7c16b5671d0e | 7 | } |
tnhnrl | 17:7c16b5671d0e | 8 | |
tnhnrl | 38:83d06c294807 | 9 | void ConfigFileIO::saveBattData(float p_gain, float i_gain, float d_gain) { |
tnhnrl | 38:83d06c294807 | 10 | ConfigFile write_Batt_txt; //write to the config file |
tnhnrl | 38:83d06c294807 | 11 | |
tnhnrl | 38:83d06c294807 | 12 | char PGain_buffer[128]; |
tnhnrl | 38:83d06c294807 | 13 | sprintf(PGain_buffer,"# configuration file for battery mover parameters\n\n#Gains\nPGain"); |
tnhnrl | 38:83d06c294807 | 14 | |
tnhnrl | 38:83d06c294807 | 15 | char string_pgain[128]; |
tnhnrl | 38:83d06c294807 | 16 | sprintf(string_pgain, "%f", p_gain); |
tnhnrl | 38:83d06c294807 | 17 | write_Batt_txt.setValue(PGain_buffer, string_pgain); |
tnhnrl | 38:83d06c294807 | 18 | |
tnhnrl | 38:83d06c294807 | 19 | char string_igain[128]; |
tnhnrl | 38:83d06c294807 | 20 | sprintf(string_igain, "%f", i_gain); |
tnhnrl | 38:83d06c294807 | 21 | write_Batt_txt.setValue("IGain", string_igain); |
tnhnrl | 38:83d06c294807 | 22 | |
tnhnrl | 38:83d06c294807 | 23 | char string_dgain[128]; |
tnhnrl | 38:83d06c294807 | 24 | sprintf(string_dgain, "%f", d_gain); |
tnhnrl | 38:83d06c294807 | 25 | write_Batt_txt.setValue("DGain", string_dgain); |
tnhnrl | 38:83d06c294807 | 26 | |
tnhnrl | 38:83d06c294807 | 27 | write_Batt_txt.setValue("\n#string pot parameters\nzeroCounts", "610"); |
tnhnrl | 38:83d06c294807 | 28 | write_Batt_txt.setValue("PistonTravelLimit", "73.0"); |
tnhnrl | 38:83d06c294807 | 29 | write_Batt_txt.setValue("slope", "0.12176"); |
tnhnrl | 38:83d06c294807 | 30 | write_Batt_txt.setValue("filterWn", "6.0"); |
tnhnrl | 38:83d06c294807 | 31 | write_Batt_txt.setValue("deadband", "0.5"); |
tnhnrl | 38:83d06c294807 | 32 | |
tnhnrl | 38:83d06c294807 | 33 | //SAVE THE DATA! |
tnhnrl | 38:83d06c294807 | 34 | pc().printf("Saving BATTERY MOVER PID data!"); |
tnhnrl | 38:83d06c294807 | 35 | |
tnhnrl | 38:83d06c294807 | 36 | if (!write_Batt_txt.write("/local/batt.txt")) { |
tnhnrl | 38:83d06c294807 | 37 | pc().printf("\n\rERROR: (SAVE)Failure to write batt.txt file."); |
tnhnrl | 38:83d06c294807 | 38 | } |
tnhnrl | 38:83d06c294807 | 39 | else { |
tnhnrl | 38:83d06c294807 | 40 | pc().printf("\n\rFile batt.txt successful written.\n\r"); |
tnhnrl | 38:83d06c294807 | 41 | } |
tnhnrl | 38:83d06c294807 | 42 | } |
tnhnrl | 38:83d06c294807 | 43 | |
tnhnrl | 21:38c8544db6f4 | 44 | void ConfigFileIO::savePitchData(float p_gain, float i_gain, float d_gain, int batt_zeroOffset) { |
tnhnrl | 21:38c8544db6f4 | 45 | ConfigFile write_pitch_txt; //write to the config file |
tnhnrl | 21:38c8544db6f4 | 46 | |
tnhnrl | 21:38c8544db6f4 | 47 | char PGain_buffer[128]; |
tnhnrl | 21:38c8544db6f4 | 48 | sprintf(PGain_buffer,"# pitch outer loop parameters\n\n#Gains\nPGain"); |
tnhnrl | 21:38c8544db6f4 | 49 | |
tnhnrl | 21:38c8544db6f4 | 50 | char string_pgain[128]; |
tnhnrl | 21:38c8544db6f4 | 51 | sprintf(string_pgain, "%f", p_gain); |
tnhnrl | 21:38c8544db6f4 | 52 | write_pitch_txt.setValue(PGain_buffer, string_pgain); |
tnhnrl | 21:38c8544db6f4 | 53 | |
tnhnrl | 21:38c8544db6f4 | 54 | char string_igain[128]; |
tnhnrl | 21:38c8544db6f4 | 55 | sprintf(string_igain, "%f", i_gain); |
tnhnrl | 21:38c8544db6f4 | 56 | write_pitch_txt.setValue("IGain", string_igain); |
tnhnrl | 17:7c16b5671d0e | 57 | |
tnhnrl | 21:38c8544db6f4 | 58 | char string_dgain[128]; |
tnhnrl | 21:38c8544db6f4 | 59 | sprintf(string_dgain, "%f", d_gain); |
tnhnrl | 21:38c8544db6f4 | 60 | write_pitch_txt.setValue("DGain", string_dgain); |
tnhnrl | 21:38c8544db6f4 | 61 | |
tnhnrl | 21:38c8544db6f4 | 62 | write_pitch_txt.setValue("\n#Pitch sensor filter parameters\nfilterWn", "6.0"); |
tnhnrl | 21:38c8544db6f4 | 63 | write_pitch_txt.setValue("deadband", "0.5"); |
tnhnrl | 17:7c16b5671d0e | 64 | |
tnhnrl | 21:38c8544db6f4 | 65 | char string_zeroOffset[128]; |
tnhnrl | 21:38c8544db6f4 | 66 | sprintf(string_zeroOffset, "%d", batt_zeroOffset); |
tnhnrl | 21:38c8544db6f4 | 67 | //bce setting was 41 mm during LASR experiments |
tnhnrl | 21:38c8544db6f4 | 68 | write_pitch_txt.setValue("\n#Offset for neutral (default: 41)\nzeroOffset", string_zeroOffset); |
tnhnrl | 21:38c8544db6f4 | 69 | |
tnhnrl | 21:38c8544db6f4 | 70 | //SAVE THE DATA! |
tnhnrl | 21:38c8544db6f4 | 71 | pc().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!"); |
tnhnrl | 21:38c8544db6f4 | 72 | |
tnhnrl | 21:38c8544db6f4 | 73 | if (!write_pitch_txt.write("/local/pitch.txt")) { |
tnhnrl | 21:38c8544db6f4 | 74 | pc().printf("\n\rERROR: (SAVE)Failure to write depth.txt file."); |
tnhnrl | 17:7c16b5671d0e | 75 | } |
tnhnrl | 17:7c16b5671d0e | 76 | else { |
tnhnrl | 21:38c8544db6f4 | 77 | pc().printf("\n\rFile pitch.txt successful written.\n\r"); |
tnhnrl | 21:38c8544db6f4 | 78 | } |
tnhnrl | 21:38c8544db6f4 | 79 | } |
tnhnrl | 21:38c8544db6f4 | 80 | |
tnhnrl | 38:83d06c294807 | 81 | void ConfigFileIO::saveBCEData(float p_gain, float i_gain, float d_gain) { |
tnhnrl | 38:83d06c294807 | 82 | ConfigFile write_BCE_txt; //write to the config file |
tnhnrl | 38:83d06c294807 | 83 | |
tnhnrl | 38:83d06c294807 | 84 | char PGain_buffer[128]; |
tnhnrl | 38:83d06c294807 | 85 | sprintf(PGain_buffer,"# configuration file for BCE parameters\n\n#Gains\nPGain"); |
tnhnrl | 38:83d06c294807 | 86 | |
tnhnrl | 38:83d06c294807 | 87 | char string_pgain[128]; |
tnhnrl | 38:83d06c294807 | 88 | sprintf(string_pgain, "%f", p_gain); |
tnhnrl | 38:83d06c294807 | 89 | write_BCE_txt.setValue(PGain_buffer, string_pgain); |
tnhnrl | 38:83d06c294807 | 90 | |
tnhnrl | 38:83d06c294807 | 91 | char string_igain[128]; |
tnhnrl | 38:83d06c294807 | 92 | sprintf(string_igain, "%f", i_gain); |
tnhnrl | 38:83d06c294807 | 93 | write_BCE_txt.setValue("IGain", string_igain); |
tnhnrl | 38:83d06c294807 | 94 | |
tnhnrl | 38:83d06c294807 | 95 | char string_dgain[128]; |
tnhnrl | 38:83d06c294807 | 96 | sprintf(string_dgain, "%f", d_gain); |
tnhnrl | 38:83d06c294807 | 97 | write_BCE_txt.setValue("DGain", string_dgain); |
tnhnrl | 38:83d06c294807 | 98 | |
tnhnrl | 38:83d06c294807 | 99 | write_BCE_txt.setValue("\n#string pot parameters\nzeroCounts", "253"); |
tnhnrl | 38:83d06c294807 | 100 | write_BCE_txt.setValue("PistonTravelLimit", "320.0"); |
tnhnrl | 38:83d06c294807 | 101 | write_BCE_txt.setValue("slope", "0.12176"); |
tnhnrl | 38:83d06c294807 | 102 | write_BCE_txt.setValue("filterWn", "6.0"); |
tnhnrl | 38:83d06c294807 | 103 | write_BCE_txt.setValue("deadband", "0.5"); |
tnhnrl | 38:83d06c294807 | 104 | |
tnhnrl | 38:83d06c294807 | 105 | //SAVE THE DATA! |
tnhnrl | 38:83d06c294807 | 106 | pc().printf("Saving BCE PID data!"); |
tnhnrl | 38:83d06c294807 | 107 | |
tnhnrl | 38:83d06c294807 | 108 | if (!write_BCE_txt.write("/local/bce.txt")) { |
tnhnrl | 38:83d06c294807 | 109 | pc().printf("\n\rERROR: (SAVE)Failure to write bce.txt file."); |
tnhnrl | 38:83d06c294807 | 110 | } |
tnhnrl | 38:83d06c294807 | 111 | else { |
tnhnrl | 38:83d06c294807 | 112 | pc().printf("\n\rFile bce.txt successful written.\n\r"); |
tnhnrl | 38:83d06c294807 | 113 | } |
tnhnrl | 38:83d06c294807 | 114 | } |
tnhnrl | 38:83d06c294807 | 115 | |
tnhnrl | 21:38c8544db6f4 | 116 | void ConfigFileIO::saveDepthData(float p_gain, float i_gain, float d_gain, int bce_zeroOffset) { |
tnhnrl | 21:38c8544db6f4 | 117 | ConfigFile write_depth_txt; //write to the config file |
tnhnrl | 21:38c8544db6f4 | 118 | |
tnhnrl | 21:38c8544db6f4 | 119 | char PGain_buffer[128]; |
tnhnrl | 21:38c8544db6f4 | 120 | sprintf(PGain_buffer,"# depth outer loop parameters\n\n#Gains\nPGain"); |
tnhnrl | 21:38c8544db6f4 | 121 | |
tnhnrl | 21:38c8544db6f4 | 122 | char string_pgain[128]; |
tnhnrl | 21:38c8544db6f4 | 123 | sprintf(string_pgain, "%f", p_gain); |
tnhnrl | 21:38c8544db6f4 | 124 | write_depth_txt.setValue(PGain_buffer, string_pgain); |
tnhnrl | 21:38c8544db6f4 | 125 | |
tnhnrl | 21:38c8544db6f4 | 126 | char string_igain[128]; |
tnhnrl | 21:38c8544db6f4 | 127 | sprintf(string_igain, "%f", i_gain); |
tnhnrl | 21:38c8544db6f4 | 128 | write_depth_txt.setValue("IGain", string_igain); |
tnhnrl | 17:7c16b5671d0e | 129 | |
tnhnrl | 21:38c8544db6f4 | 130 | char string_dgain[128]; |
tnhnrl | 21:38c8544db6f4 | 131 | sprintf(string_dgain, "%f", d_gain); |
tnhnrl | 21:38c8544db6f4 | 132 | write_depth_txt.setValue("DGain", string_dgain); |
tnhnrl | 21:38c8544db6f4 | 133 | |
tnhnrl | 21:38c8544db6f4 | 134 | write_depth_txt.setValue("\n#Depth sensor filter parameters\nfilterWn", "6.0"); |
tnhnrl | 21:38c8544db6f4 | 135 | write_depth_txt.setValue("deadband", "0.5"); |
tnhnrl | 17:7c16b5671d0e | 136 | |
tnhnrl | 21:38c8544db6f4 | 137 | char string_zeroOffset[128]; |
tnhnrl | 21:38c8544db6f4 | 138 | sprintf(string_zeroOffset, "%d", bce_zeroOffset); |
tnhnrl | 21:38c8544db6f4 | 139 | //bce setting was 240 mm during LASR experiments |
tnhnrl | 21:38c8544db6f4 | 140 | write_depth_txt.setValue("\n#Offset for neutral (default: 240)\nzeroOffset", string_zeroOffset); |
tnhnrl | 21:38c8544db6f4 | 141 | |
tnhnrl | 21:38c8544db6f4 | 142 | //SAVE THE DATA! |
tnhnrl | 21:38c8544db6f4 | 143 | pc().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!"); |
tnhnrl | 21:38c8544db6f4 | 144 | |
tnhnrl | 21:38c8544db6f4 | 145 | if (!write_depth_txt.write("/local/depth.txt")) { |
tnhnrl | 21:38c8544db6f4 | 146 | pc().printf("\n\rERROR: (SAVE)Failure to write depth.txt file."); |
tnhnrl | 17:7c16b5671d0e | 147 | } |
tnhnrl | 17:7c16b5671d0e | 148 | else { |
tnhnrl | 21:38c8544db6f4 | 149 | pc().printf("\n\rFile depth.txt successful written.\n\r"); |
tnhnrl | 17:7c16b5671d0e | 150 | } |
tnhnrl | 17:7c16b5671d0e | 151 | } |
tnhnrl | 17:7c16b5671d0e | 152 | |
tnhnrl | 21:38c8544db6f4 | 153 | int ConfigFileIO::load_BCE_config() { |
tnhnrl | 21:38c8544db6f4 | 154 | ConfigFile cfg; |
tnhnrl | 21:38c8544db6f4 | 155 | int count = 0; |
tnhnrl | 21:38c8544db6f4 | 156 | if (!cfg.read("/local/bce.txt")) { |
tnhnrl | 21:38c8544db6f4 | 157 | error("File Read Error"); |
tnhnrl | 21:38c8544db6f4 | 158 | } |
tnhnrl | 21:38c8544db6f4 | 159 | char value[BUFSIZ]; |
tnhnrl | 21:38c8544db6f4 | 160 | |
tnhnrl | 21:38c8544db6f4 | 161 | if (cfg.getValue("PGain", &value[0] , sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 162 | bce().setControllerP(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 163 | count++; |
tnhnrl | 21:38c8544db6f4 | 164 | } |
tnhnrl | 21:38c8544db6f4 | 165 | if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 166 | bce().setControllerI(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 167 | count++; |
tnhnrl | 21:38c8544db6f4 | 168 | } |
tnhnrl | 21:38c8544db6f4 | 169 | if (cfg.getValue("DGain", &value[0] , sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 170 | bce().setControllerD(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 171 | count++; |
tnhnrl | 17:7c16b5671d0e | 172 | } |
tnhnrl | 21:38c8544db6f4 | 173 | if (cfg.getValue("zeroCounts", &value[0],sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 174 | bce().setZeroCounts(atoi(value)); |
tnhnrl | 21:38c8544db6f4 | 175 | count++; |
tnhnrl | 21:38c8544db6f4 | 176 | } |
tnhnrl | 21:38c8544db6f4 | 177 | if (cfg.getValue("PistonTravelLimit", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 178 | bce().setTravelLimit(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 179 | count++; |
tnhnrl | 21:38c8544db6f4 | 180 | } |
tnhnrl | 21:38c8544db6f4 | 181 | if (cfg.getValue("slope", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 182 | bce().setPotSlope(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 183 | count++; |
tnhnrl | 21:38c8544db6f4 | 184 | } |
tnhnrl | 21:38c8544db6f4 | 185 | if (cfg.getValue("filterWn", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 186 | bce().setFilterFrequency(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 187 | count++; |
tnhnrl | 21:38c8544db6f4 | 188 | } |
tnhnrl | 21:38c8544db6f4 | 189 | if (cfg.getValue("deadband", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 190 | bce().setDeadband(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 191 | count++; |
tnhnrl | 21:38c8544db6f4 | 192 | } |
tnhnrl | 17:7c16b5671d0e | 193 | |
tnhnrl | 21:38c8544db6f4 | 194 | return count; |
tnhnrl | 17:7c16b5671d0e | 195 | } |
tnhnrl | 17:7c16b5671d0e | 196 | |
tnhnrl | 21:38c8544db6f4 | 197 | int ConfigFileIO::load_BATT_config() { |
tnhnrl | 21:38c8544db6f4 | 198 | ConfigFile cfg; |
tnhnrl | 21:38c8544db6f4 | 199 | int count = 0; |
tnhnrl | 21:38c8544db6f4 | 200 | if (!cfg.read("/local/batt.txt")) { |
tnhnrl | 21:38c8544db6f4 | 201 | error("File Read Error"); |
tnhnrl | 21:38c8544db6f4 | 202 | } |
tnhnrl | 21:38c8544db6f4 | 203 | char value[BUFSIZ]; |
tnhnrl | 21:38c8544db6f4 | 204 | |
tnhnrl | 21:38c8544db6f4 | 205 | |
tnhnrl | 21:38c8544db6f4 | 206 | if (cfg.getValue("PGain", &value[0] , sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 207 | batt().setControllerP(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 208 | count++; |
tnhnrl | 21:38c8544db6f4 | 209 | } |
tnhnrl | 21:38c8544db6f4 | 210 | if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 211 | batt().setControllerI(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 212 | count++; |
tnhnrl | 21:38c8544db6f4 | 213 | } |
tnhnrl | 21:38c8544db6f4 | 214 | if (cfg.getValue("DGain", &value[0] , sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 215 | batt().setControllerD(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 216 | count++; |
tnhnrl | 21:38c8544db6f4 | 217 | } |
tnhnrl | 21:38c8544db6f4 | 218 | if (cfg.getValue("zeroCounts", &value[0],sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 219 | batt().setZeroCounts(atoi(value)); |
tnhnrl | 21:38c8544db6f4 | 220 | count++; |
tnhnrl | 21:38c8544db6f4 | 221 | } |
tnhnrl | 21:38c8544db6f4 | 222 | if (cfg.getValue("PistonTravelLimit", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 223 | batt().setTravelLimit(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 224 | count++; |
tnhnrl | 21:38c8544db6f4 | 225 | } |
tnhnrl | 21:38c8544db6f4 | 226 | if (cfg.getValue("slope", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 227 | batt().setPotSlope(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 228 | count++; |
tnhnrl | 21:38c8544db6f4 | 229 | } |
tnhnrl | 21:38c8544db6f4 | 230 | if (cfg.getValue("filterWn", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 231 | batt().setFilterFrequency(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 232 | count++; |
tnhnrl | 21:38c8544db6f4 | 233 | } |
tnhnrl | 21:38c8544db6f4 | 234 | if (cfg.getValue("deadband", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 235 | batt().setDeadband(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 236 | count++; |
tnhnrl | 21:38c8544db6f4 | 237 | } |
tnhnrl | 21:38c8544db6f4 | 238 | |
tnhnrl | 21:38c8544db6f4 | 239 | return count; |
tnhnrl | 21:38c8544db6f4 | 240 | } |
tnhnrl | 17:7c16b5671d0e | 241 | |
tnhnrl | 21:38c8544db6f4 | 242 | int ConfigFileIO::load_DEPTH_config() { |
tnhnrl | 21:38c8544db6f4 | 243 | ConfigFile cfg; |
tnhnrl | 21:38c8544db6f4 | 244 | int count = 0; |
tnhnrl | 21:38c8544db6f4 | 245 | if (!cfg.read("/local/depth.txt")) { |
tnhnrl | 21:38c8544db6f4 | 246 | error("File Read Error"); |
tnhnrl | 21:38c8544db6f4 | 247 | } |
tnhnrl | 21:38c8544db6f4 | 248 | char value[BUFSIZ]; |
tnhnrl | 21:38c8544db6f4 | 249 | |
tnhnrl | 21:38c8544db6f4 | 250 | if (cfg.getValue("PGain", &value[0] , sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 251 | depthLoop().setControllerP(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 252 | count++; |
tnhnrl | 21:38c8544db6f4 | 253 | } |
tnhnrl | 21:38c8544db6f4 | 254 | if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 255 | depthLoop().setControllerI(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 256 | count++; |
tnhnrl | 21:38c8544db6f4 | 257 | } |
tnhnrl | 21:38c8544db6f4 | 258 | if (cfg.getValue("DGain", &value[0] , sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 259 | depthLoop().setControllerD(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 260 | count++; |
tnhnrl | 21:38c8544db6f4 | 261 | } |
tnhnrl | 21:38c8544db6f4 | 262 | if (cfg.getValue("filterWn", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 263 | depthLoop().setFilterFrequency(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 264 | count++; |
tnhnrl | 21:38c8544db6f4 | 265 | } |
tnhnrl | 21:38c8544db6f4 | 266 | if (cfg.getValue("deadband", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 267 | depthLoop().setDeadband(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 268 | count++; |
tnhnrl | 21:38c8544db6f4 | 269 | } |
tnhnrl | 21:38c8544db6f4 | 270 | if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 271 | depthLoop().setOutputOffset(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 272 | count++; |
tnhnrl | 21:38c8544db6f4 | 273 | } |
tnhnrl | 21:38c8544db6f4 | 274 | return count; |
tnhnrl | 21:38c8544db6f4 | 275 | } |
tnhnrl | 17:7c16b5671d0e | 276 | |
tnhnrl | 21:38c8544db6f4 | 277 | int ConfigFileIO::load_PITCH_config() { |
tnhnrl | 21:38c8544db6f4 | 278 | ConfigFile cfg; |
tnhnrl | 21:38c8544db6f4 | 279 | int count = 0; |
tnhnrl | 21:38c8544db6f4 | 280 | if (!cfg.read("/local/pitch.txt")){ |
tnhnrl | 21:38c8544db6f4 | 281 | error("File Read Error"); |
tnhnrl | 21:38c8544db6f4 | 282 | } |
tnhnrl | 21:38c8544db6f4 | 283 | char value[BUFSIZ]; |
tnhnrl | 21:38c8544db6f4 | 284 | |
tnhnrl | 21:38c8544db6f4 | 285 | if (cfg.getValue("PGain", &value[0] , sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 286 | pitchLoop().setControllerP(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 287 | count++; |
tnhnrl | 21:38c8544db6f4 | 288 | } |
tnhnrl | 21:38c8544db6f4 | 289 | if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 290 | pitchLoop().setControllerI(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 291 | count++; |
tnhnrl | 21:38c8544db6f4 | 292 | } |
tnhnrl | 21:38c8544db6f4 | 293 | if (cfg.getValue("DGain", &value[0] , sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 294 | pitchLoop().setControllerD(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 295 | count++; |
tnhnrl | 21:38c8544db6f4 | 296 | } |
tnhnrl | 21:38c8544db6f4 | 297 | if (cfg.getValue("filterWn", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 298 | pitchLoop().setFilterFrequency(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 299 | count++; |
tnhnrl | 21:38c8544db6f4 | 300 | } |
tnhnrl | 21:38c8544db6f4 | 301 | if (cfg.getValue("deadband", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 302 | pitchLoop().setDeadband(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 303 | count++; |
tnhnrl | 21:38c8544db6f4 | 304 | } |
tnhnrl | 21:38c8544db6f4 | 305 | |
tnhnrl | 21:38c8544db6f4 | 306 | if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) { |
tnhnrl | 21:38c8544db6f4 | 307 | pitchLoop().setOutputOffset(atof(value)); |
tnhnrl | 21:38c8544db6f4 | 308 | count++; |
tnhnrl | 21:38c8544db6f4 | 309 | } |
tnhnrl | 21:38c8544db6f4 | 310 | return count; |
tnhnrl | 43:891baf306e0a | 311 | } |
tnhnrl | 43:891baf306e0a | 312 | |
tnhnrl | 43:891baf306e0a | 313 | int ConfigFileIO::load_RUDDER_config() { |
tnhnrl | 43:891baf306e0a | 314 | ConfigFile cfg; |
tnhnrl | 43:891baf306e0a | 315 | int count = 0; |
tnhnrl | 43:891baf306e0a | 316 | if (!cfg.read("/local/rudder.txt")){ |
tnhnrl | 43:891baf306e0a | 317 | error("File Read Error"); |
tnhnrl | 43:891baf306e0a | 318 | } |
tnhnrl | 43:891baf306e0a | 319 | char value[BUFSIZ]; |
tnhnrl | 43:891baf306e0a | 320 | |
tnhnrl | 43:891baf306e0a | 321 | if (cfg.getValue("PGain", &value[0] , sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 322 | rudderLoop().setControllerP(atof(value)); |
tnhnrl | 43:891baf306e0a | 323 | count++; |
tnhnrl | 43:891baf306e0a | 324 | } |
tnhnrl | 43:891baf306e0a | 325 | if (cfg.getValue("IGain", &value[0] ,sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 326 | rudderLoop().setControllerI(atof(value)); |
tnhnrl | 43:891baf306e0a | 327 | count++; |
tnhnrl | 43:891baf306e0a | 328 | } |
tnhnrl | 43:891baf306e0a | 329 | if (cfg.getValue("DGain", &value[0] , sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 330 | rudderLoop().setControllerD(atof(value)); |
tnhnrl | 43:891baf306e0a | 331 | count++; |
tnhnrl | 43:891baf306e0a | 332 | } |
tnhnrl | 43:891baf306e0a | 333 | if (cfg.getValue("filterWn", &value[0], sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 334 | rudderLoop().setFilterFrequency(atof(value)); |
tnhnrl | 43:891baf306e0a | 335 | count++; |
tnhnrl | 43:891baf306e0a | 336 | } |
tnhnrl | 43:891baf306e0a | 337 | if (cfg.getValue("deadband", &value[0], sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 338 | rudderLoop().setDeadband(atof(value)); |
tnhnrl | 43:891baf306e0a | 339 | count++; |
tnhnrl | 43:891baf306e0a | 340 | } |
tnhnrl | 43:891baf306e0a | 341 | |
tnhnrl | 43:891baf306e0a | 342 | if (cfg.getValue("zeroOffset", &value[0], sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 343 | rudderLoop().setOutputOffset(atof(value)); |
tnhnrl | 43:891baf306e0a | 344 | count++; |
tnhnrl | 43:891baf306e0a | 345 | } |
tnhnrl | 43:891baf306e0a | 346 | return count; |
tnhnrl | 43:891baf306e0a | 347 | } |
tnhnrl | 43:891baf306e0a | 348 | |
tnhnrl | 43:891baf306e0a | 349 | int ConfigFileIO::load_SERVO_config() { |
tnhnrl | 43:891baf306e0a | 350 | ConfigFile cfg; |
tnhnrl | 43:891baf306e0a | 351 | int count = 0; |
tnhnrl | 43:891baf306e0a | 352 | if (!cfg.read("/local/servo.txt")){ |
tnhnrl | 43:891baf306e0a | 353 | error("File Read Error"); |
tnhnrl | 43:891baf306e0a | 354 | } |
tnhnrl | 43:891baf306e0a | 355 | char value[BUFSIZ]; |
tnhnrl | 43:891baf306e0a | 356 | |
tnhnrl | 43:891baf306e0a | 357 | if (cfg.getValue("setMinDeg", &value[0] , sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 358 | servo().setMinDeg(atof(value)); |
tnhnrl | 43:891baf306e0a | 359 | count++; |
tnhnrl | 43:891baf306e0a | 360 | } |
tnhnrl | 43:891baf306e0a | 361 | if (cfg.getValue("setMaxDeg", &value[0] ,sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 362 | servo().setMaxDeg(atof(value)); |
tnhnrl | 43:891baf306e0a | 363 | count++; |
tnhnrl | 43:891baf306e0a | 364 | } |
tnhnrl | 43:891baf306e0a | 365 | if (cfg.getValue("setCenterPMW", &value[0] , sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 366 | servo().setCenterPWM(atof(value)); |
tnhnrl | 43:891baf306e0a | 367 | count++; |
tnhnrl | 43:891baf306e0a | 368 | } |
tnhnrl | 43:891baf306e0a | 369 | if (cfg.getValue("setMinPWM", &value[0], sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 370 | servo().setMinPWM(atof(value)); |
tnhnrl | 43:891baf306e0a | 371 | count++; |
tnhnrl | 43:891baf306e0a | 372 | } |
tnhnrl | 43:891baf306e0a | 373 | if (cfg.getValue("setMaxPWM", &value[0], sizeof(value))) { |
tnhnrl | 43:891baf306e0a | 374 | servo().setMaxPWM(atof(value)); |
tnhnrl | 43:891baf306e0a | 375 | count++; |
tnhnrl | 43:891baf306e0a | 376 | } |
tnhnrl | 43:891baf306e0a | 377 | return count; |
tnhnrl | 21:38c8544db6f4 | 378 | } |