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
main.cpp@27:0a5b90cd65d6, 2017-11-29 (annotated)
- Committer:
- tnhnrl
- Date:
- Wed Nov 29 22:37:47 2017 +0000
- Revision:
- 27:0a5b90cd65d6
- Parent:
- 24:c7d9b5bf3829
- Child:
- 28:16c83a2fdefa
depthTolerance variable removed 11/29
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
danstrider | 10:085ab7328054 | 1 | /* |
tnhnrl | 24:c7d9b5bf3829 | 2 | Modified 2017-11-29 revA by Troy |
tnhnrl | 24:c7d9b5bf3829 | 3 | - changelog.txt carries previous changes |
tnhnrl | 24:c7d9b5bf3829 | 4 | - Incorporated email changes and did quick bench test |
tnhnrl | 24:c7d9b5bf3829 | 5 | - Redid the sub-state recording method to only update on state changes |
tnhnrl | 27:0a5b90cd65d6 | 6 | Modified 2017-11-29 revB by Troy |
tnhnrl | 27:0a5b90cd65d6 | 7 | - minor print fixes |
danstrider | 10:085ab7328054 | 8 | */ |
tnhnrl | 20:8987a9ae2bc7 | 9 | |
tzyoung | 0:ea293bbf9717 | 10 | #include "mbed.h" |
tzyoung | 0:ea293bbf9717 | 11 | #include "StaticDefs.hpp" |
tnhnrl | 20:8987a9ae2bc7 | 12 | |
tnhnrl | 20:8987a9ae2bc7 | 13 | // loop rate timer for slowing down how fast while(1) runs in main() |
tnhnrl | 20:8987a9ae2bc7 | 14 | Ticker loop_rate_ticker; |
tnhnrl | 20:8987a9ae2bc7 | 15 | volatile bool loop = false; |
tnhnrl | 20:8987a9ae2bc7 | 16 | void loop_trigger() { loop = true; }; // loop trigger |
tnhnrl | 20:8987a9ae2bc7 | 17 | |
danstrider | 10:085ab7328054 | 18 | void setup() { |
danstrider | 11:3b241ecb75ed | 19 | pc().baud(57600); |
tnhnrl | 27:0a5b90cd65d6 | 20 | pc().printf("\n\n\rFSG POOL TEST 2017-11-29 revB\n\n\r"); |
tnhnrl | 20:8987a9ae2bc7 | 21 | |
danstrider | 10:085ab7328054 | 22 | // start up the system timer |
tzyoung | 0:ea293bbf9717 | 23 | systemTime().start(); |
tnhnrl | 20:8987a9ae2bc7 | 24 | |
danstrider | 10:085ab7328054 | 25 | // set up and start the adc. This runs on a fixed interval and is interrupt driven |
tzyoung | 0:ea293bbf9717 | 26 | adc().initialize(); |
tzyoung | 0:ea293bbf9717 | 27 | adc().start(); |
danstrider | 10:085ab7328054 | 28 | |
danstrider | 10:085ab7328054 | 29 | // set up and start the imu. This polls in the background |
danstrider | 10:085ab7328054 | 30 | imu().initialize(); |
danstrider | 10:085ab7328054 | 31 | imu().start(); |
danstrider | 10:085ab7328054 | 32 | |
danstrider | 10:085ab7328054 | 33 | // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298 |
danstrider | 14:85b64a4d08e8 | 34 | depth().init(); |
danstrider | 14:85b64a4d08e8 | 35 | depth().tare(); |
danstrider | 10:085ab7328054 | 36 | |
danstrider | 10:085ab7328054 | 37 | // construct a local file system |
danstrider | 10:085ab7328054 | 38 | local(); |
tnhnrl | 20:8987a9ae2bc7 | 39 | |
danstrider | 10:085ab7328054 | 40 | // load config data from files |
tnhnrl | 21:38c8544db6f4 | 41 | configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt" |
tnhnrl | 21:38c8544db6f4 | 42 | configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt" |
tnhnrl | 21:38c8544db6f4 | 43 | configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position) |
tnhnrl | 21:38c8544db6f4 | 44 | configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position) |
tnhnrl | 20:8987a9ae2bc7 | 45 | |
danstrider | 10:085ab7328054 | 46 | // set up the linear actuators. adc has to be running first. |
danstrider | 10:085ab7328054 | 47 | bce().init(); |
danstrider | 10:085ab7328054 | 48 | bce().start(); |
mkelly10 | 12:a0519d11d2b6 | 49 | bce().pause(); // start by not moving |
tnhnrl | 20:8987a9ae2bc7 | 50 | |
danstrider | 10:085ab7328054 | 51 | batt().init(); |
tzyoung | 2:892b58e56712 | 52 | batt().start(); |
mkelly10 | 12:a0519d11d2b6 | 53 | batt().pause(); // start by not moving |
tnhnrl | 20:8987a9ae2bc7 | 54 | |
danstrider | 10:085ab7328054 | 55 | // set up the depth and pitch outer loop controllers |
danstrider | 10:085ab7328054 | 56 | depthLoop().init(); |
tnhnrl | 13:84fcbe1dcd62 | 57 | depthLoop().start(); |
tnhnrl | 16:3363b9f14913 | 58 | depthLoop().setCommand(stateMachine().getDepthCommand()); |
tnhnrl | 20:8987a9ae2bc7 | 59 | |
danstrider | 10:085ab7328054 | 60 | pitchLoop().init(); |
tnhnrl | 13:84fcbe1dcd62 | 61 | pitchLoop().start(); |
tnhnrl | 16:3363b9f14913 | 62 | pitchLoop().setCommand(stateMachine().getPitchCommand()); |
tnhnrl | 20:8987a9ae2bc7 | 63 | |
tnhnrl | 20:8987a9ae2bc7 | 64 | // show that the PID gains are loading from the file |
tnhnrl | 21:38c8544db6f4 | 65 | pc().printf("bce P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope()); |
tnhnrl | 21:38c8544db6f4 | 66 | pc().printf("batt P:%6.2f, I:%6.2f, D:%6.2f, zero %3i, limit %3.0f mm, slope %3.3f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope()); |
tnhnrl | 21:38c8544db6f4 | 67 | pc().printf("depth P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset()); |
tnhnrl | 21:38c8544db6f4 | 68 | pc().printf("pitch P:%6.2f, I:%6.2f, D:%6.2f, offset:%6.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset()); |
danstrider | 10:085ab7328054 | 69 | pc().printf("\n\r"); |
tnhnrl | 21:38c8544db6f4 | 70 | |
tnhnrl | 17:7c16b5671d0e | 71 | //load sequence from file |
tnhnrl | 17:7c16b5671d0e | 72 | sequenceController().loadSequence(); |
tnhnrl | 20:8987a9ae2bc7 | 73 | |
tnhnrl | 20:8987a9ae2bc7 | 74 | // establish the main loop rate |
tnhnrl | 20:8987a9ae2bc7 | 75 | loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate |
tnhnrl | 21:38c8544db6f4 | 76 | } |
tnhnrl | 20:8987a9ae2bc7 | 77 | |
danstrider | 10:085ab7328054 | 78 | int main() { |
danstrider | 10:085ab7328054 | 79 | setup(); |
tnhnrl | 16:3363b9f14913 | 80 | |
danstrider | 10:085ab7328054 | 81 | while(1) { |
tnhnrl | 20:8987a9ae2bc7 | 82 | // does this stuff at the loop rate |
tnhnrl | 20:8987a9ae2bc7 | 83 | if(loop) { |
tnhnrl | 20:8987a9ae2bc7 | 84 | led1() = !led1(); // blink |
tnhnrl | 20:8987a9ae2bc7 | 85 | stateMachine().runStateMachine(); |
tnhnrl | 20:8987a9ae2bc7 | 86 | loop = false; // wait until the loop rate timer fires again |
tnhnrl | 17:7c16b5671d0e | 87 | } |
danstrider | 10:085ab7328054 | 88 | } |
danstrider | 10:085ab7328054 | 89 | } |