Diff: main.cpp

Revision:

13:84fcbe1dcd62

Parent:

12:a0519d11d2b6

Child:

14:85b64a4d08e8

--- a/main.cpp	Mon Oct 30 19:46:38 2017 +0000
+++ b/main.cpp	Mon Oct 30 21:09:19 2017 +0000
@@ -109,9 +109,11 @@
 
     // set up the depth and pitch outer loop controllers
     depthLoop().init();
+    depthLoop().start();
     depthLoop().setCommand(-2.5);
 
     pitchLoop().init();
+    pitchLoop().start();
     pitchLoop().setCommand(0.0);
 
     // do not leave this in. Check that PID gains are loading
@@ -503,7 +505,7 @@
         }
         
         else if (userInput == 'C' or userInput == 'c') {
-            pc().printf("depth: %3.1f\r\n",depth().getDepth());
+            pc().printf("depth: %3.1f\r\n",depthLoop().getPosition());
             pc().printf("pitch: %3.1f\r\n",imu().getPitch());
             pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
             pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
@@ -535,8 +537,6 @@
             // what is active?
             bce().pause();
             batt().pause();
-//            depthLoop().stop();
-//            pitchLoop().stop();
         }
         // how exit?
         if (pc().readable()) {
@@ -579,8 +579,8 @@
             timer.reset();
             isTimeoutRunning = false;
         }
-        else if (depth().getDepth() < 0.2) {
-            pc().printf("depth: %3.1f, cmd: 0.5\r\n",depth().getDepth());
+        else if (depthLoop().getPosition() < 0.2) {
+            pc().printf("depth: %3.1f, cmd: 0.5\r\n",depthLoop().getPosition());
             state = FLOAT_LEVEL;
             timer.reset();
             isTimeoutRunning = false;
@@ -598,9 +598,7 @@
             // what needs to be started?
             bce().unpause();
             batt().unpause();
- //           depthLoop().start();
- //           pitchLoop().start();
-            
+
             // what is active?
             depthLoop().setCommand(depthCommand);
             pitchLoop().setCommand(pitchCommand);
@@ -612,14 +610,14 @@
             timer.reset();
             isTimeoutRunning = false;
         }
-        else if ((abs(depth().getDepth() - depthLoop().getCommand()) < depthTolerance) and
+        else if ((abs(depthLoop().getPosition() - depthLoop().getCommand()) < depthTolerance) and
                   (abs(imu().getPitch() - pitchLoop().getCommand()) < pitchTolerance)) {
             state = RISE;
             timer.reset();
             isTimeoutRunning = false;
         }
         // what is active?
-        pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm \r", bce().getPosition_mm(), batt().getPosition_mm());
+        pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition());
         bce().setPosition_mm(depthLoop().getOutput());
         batt().setPosition_mm(pitchLoop().getOutput());
         break;
@@ -635,8 +633,6 @@
             // what needs to be started?
             bce().unpause();
             batt().unpause();
-//            depthLoop().start();
-//            pitchLoop().start();
 
             // what are the commands?
             depthLoop().setCommand(depthCommand);
@@ -651,14 +647,14 @@
             timer.reset();
             isTimeoutRunning = false;
         }
-        else if (depth().getDepth() > depthLoop().getCommand()) {
-            pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depth().getDepth(), depthLoop().getCommand());
+        else if (depthLoop().getPosition() > depthLoop().getCommand()) {
+            pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
             state = RISE;
             timer.reset();
             isTimeoutRunning = false;
         }
         // what is active?
-        pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm \r", bce().getPosition_mm(), batt().getPosition_mm());
+        pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition());
         bce().setPosition_mm(depthLoop().getOutput());
         batt().setPosition_mm(pitchLoop().getOutput());
         break;
@@ -674,9 +670,7 @@
             // what needs to be started?
             bce().unpause();
             batt().unpause();
- //           depthLoop().start();
- //           pitchLoop().start();
-
+ 
             // what are the commands?
             depthLoop().setCommand(0.0);
             pitchLoop().setCommand(-pitchCommand);
@@ -690,13 +684,13 @@
             timer.reset();
             isTimeoutRunning = false;
         }
-        else if (depth().getDepth() < depthLoop().getCommand()) {
-            pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depth().getDepth(), depthLoop().getCommand());
+        else if (depthLoop().getPosition() < depthLoop().getCommand()) {
+            pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
             state = FLOAT_LEVEL;
             timer.reset();
             isTimeoutRunning = false;
         }
-        pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm \r", bce().getPosition_mm(), batt().getPosition_mm());
+        pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition());
         // what is active?
         bce().setPosition_mm(depthLoop().getOutput());
         batt().setPosition_mm(pitchLoop().getOutput());
@@ -713,8 +707,7 @@
             // what needs to be started?
             bce().unpause();
             batt().unpause();
- //           pitchLoop().start();
-
+ 
             // what are the commands
             bce().setPosition_mm(bceFloatPosition);
             pitchLoop().setCommand(0.0);
@@ -766,7 +759,7 @@
             timer.reset();
             isTimeoutRunning = false;
         }
-        pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm \r", bce().getPosition_mm(), batt().getPosition_mm());
+        pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition());
         break;
     
     default :
@@ -781,6 +774,5 @@
         led1() = !led1(); // blink
         //pc().printf("roll: %3.1f, pitch %3.1f, yaw: %3.1f\r", imu().getRoll(), imu().getPitch(), imu().getHeading());
         stateMachine();
-        wait(0.2); // must have SOME wait or the outer loops in DIVE and RISE will lock the mbed
     }
 }
\ No newline at end of file