update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
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