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

Revision:
74:d281aaef9766
Parent:
73:f6f378311c8d
Child:
76:c802e1da4179
--- a/ConfigFileIO/ConfigFileIO.cpp	Mon Jul 30 16:48:48 2018 +0000
+++ b/ConfigFileIO/ConfigFileIO.cpp	Tue Aug 14 21:06:48 2018 +0000
@@ -41,13 +41,13 @@
     write_Batt_txt.setValue("deadband", string_deadband);
 
     //SAVE THE DATA!
-    pc().printf("Saving BATTERY MOVER PID data!");
+    xbee().printf("Saving BATTERY MOVER PID data!");
     
     if (!write_Batt_txt.write("/local/batt.txt")) {
-        pc().printf("\n\rERROR: (SAVE)Failure to write batt.txt file.");
+        xbee().printf("\n\rERROR: (SAVE)Failure to write batt.txt file.");
     }
     else {
-        pc().printf("\n\rFile batt.txt successful written.\n\r");
+        xbee().printf("\n\rFile batt.txt successful written.\n\r");
     }  
 }
 
@@ -83,13 +83,13 @@
     write_pitch_txt.setValue("\n#Offset for neutral (default: 41)\nzeroOffset", string_zeroOffset);
     
     //SAVE THE DATA!
-    pc().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!");
+    xbee().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.");
+        xbee().printf("\n\rERROR: (SAVE)Failure to write depth.txt file.");
     }
     else {
-        pc().printf("\n\rFile pitch.txt successful written.\n\r");
+        xbee().printf("\n\rFile pitch.txt successful written.\n\r");
     } 
 }
 
@@ -128,13 +128,13 @@
     write_BCE_txt.setValue("deadband", string_deadband);
 
     //SAVE THE DATA!
-    pc().printf("Saving BCE PID data!");
+    xbee().printf("Saving BCE PID data!");
     
     if (!write_BCE_txt.write("/local/bce.txt")) {
-        pc().printf("\n\rERROR: (SAVE)Failure to write bce.txt file.");
+        xbee().printf("\n\rERROR: (SAVE)Failure to write bce.txt file.");
     }
     else {
-        pc().printf("\n\rFile bce.txt successful written.\n\r");
+        xbee().printf("\n\rFile bce.txt successful written.\n\r");
     } 
 }
 
@@ -170,13 +170,13 @@
     write_depth_txt.setValue("\n#Offset for neutral (default: 240)\nzeroOffset", string_zeroOffset);
     
     //SAVE THE DATA!
-    pc().printf("Saving Buoyancy Engine Neutral Buoyancy Positions!");
+    xbee().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.");
+        xbee().printf("\n\rERROR: (SAVE)Failure to write depth.txt file.");
     }
     else {
-        pc().printf("\n\rFile depth.txt successful written.\n\r");
+        xbee().printf("\n\rFile depth.txt successful written.\n\r");
     } 
 }
 
@@ -208,13 +208,13 @@
     rudder_txt.setValue("setMaxPWM", string_max_pwm);
 
     //SAVE THE DATA!
-    pc().printf("Saving RUDDER DATA!");
+    xbee().printf("Saving RUDDER DATA!");
     
     if (!rudder_txt.write("/local/rudder.txt")) {
-        pc().printf("\n\rERROR: (SAVE)Failure to write rudder.txt file.");
+        xbee().printf("\n\rERROR: (SAVE)Failure to write rudder.txt file.");
     }
     else {
-        pc().printf("\n\rFile rudder.txt successful written.\n\r");
+        xbee().printf("\n\rFile rudder.txt successful written.\n\r");
     } 
 }
 
@@ -281,22 +281,27 @@
     char string_dgain[128];  
     sprintf(string_dgain, "%f", heading_d_gain);
     heading_txt.setValue("DGain", string_dgain);
-            
-    heading_txt.setValue("\n# HEADING sensor filter parameters\nfilterWn", "6.0");
-    heading_txt.setValue("deadband", "0.5");
+
+    char string_heading_freq[128];
+    sprintf(string_heading_freq, "%f", heading_filter_freq);
+    heading_txt.setValue("\n# HEADING sensor filter parameters\nfilterWn", string_heading_freq);
+    
+    char string_heading_db[128];
+    sprintf(string_heading_db, "%f", heading_deadband);
+    heading_txt.setValue("deadband",string_heading_db);
     
     char string_zeroOffset[128];
     sprintf(string_zeroOffset, "%f", heading_zeroOffset);
-    heading_txt.setValue("\n#HEADING offset for dive (no default!)\nzeroOffset", string_zeroOffset);
+    heading_txt.setValue("\n#HEADING offset\nzeroOffset", string_zeroOffset);
     
     //SAVE THE DATA!
-    pc().printf("(ConfigFileIO) Saving HEADING parameters!");
+    xbee().printf("(ConfigFileIO) Saving HEADING parameters!");
     
     if (!heading_txt.write("/local/heading.txt")) {
-        pc().printf("\n\rERROR: (SAVE) Failure to write heading.txt file.");
+        xbee().printf("\n\rERROR: (SAVE) Failure to write heading.txt file.");
     }
     else {
-        pc().printf("\n\rFile heading.txt successful written.\n\r");
+        xbee().printf("\n\rFile heading.txt successful written.\n\r");
     } 
 }