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:
92:52a91656458a
Parent:
59:4c04d5c7aed1
--- a/PidController/PidController.cpp	Thu May 09 14:57:45 2019 +0000
+++ b/PidController/PidController.cpp	Mon May 13 19:25:26 2019 +0000
@@ -21,9 +21,9 @@
     // hack to get heading update correct
     // need to make sure this makes physical sense
     if (_headingFlag) {
-        if (_error >= 180){
+        if (_error >= 180){  //technincally, this adjustment should continue until error is less than 180.
             _error = _error - 360.0;
-        }else if(_error <= -180){
+        }else if(_error <= -180){    // likewise here, continue until error greater than -180
             _error = _error + 360.0;
         }
     }