update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
main.cpp@28:16c83a2fdefa, 2017-12-01 (annotated)
- Committer:
- tnhnrl
- Date:
- Fri Dec 01 22:43:14 2017 +0000
- Revision:
- 28:16c83a2fdefa
- Parent:
- 27:0a5b90cd65d6
- Child:
- 30:2964617e7676
Pool tested code from Friday 12/1/17
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 | 28:16c83a2fdefa | 5 | 1) In FIND_NEUTRAL, add bce().setPosition_mm(bceFloatPosition) to the one-shot actions after the unpause(). |
tnhnrl | 28:16c83a2fdefa | 6 | 2) NEUTRAL_FIRST_PITCH, changed batt position by half from 1.0 to 0.5. |
tnhnrl | 28:16c83a2fdefa | 7 | 3) NEUTRAL_SINKING and NEUTRAL_SLOWLY_RISE are using getSetPosition now |
tnhnrl | 28:16c83a2fdefa | 8 | 4) previousPosition_mm deleted (and print statements changed) |
tnhnrl | 28:16c83a2fdefa | 9 | 5) NEUTRAL_SLOWLY_RISE check is using bce().getSetPosition_mm() |
tnhnrl | 28:16c83a2fdefa | 10 | 6) Timers replaced with _neutral_timer variable |
tnhnrl | 28:16c83a2fdefa | 11 | 7) Default timeout is 8 minutes = 480 seconds |
tnhnrl | 24:c7d9b5bf3829 | 12 | - Redid the sub-state recording method to only update on state changes |
tnhnrl | 27:0a5b90cd65d6 | 13 | Modified 2017-11-29 revB by Troy |
tnhnrl | 27:0a5b90cd65d6 | 14 | - minor print fixes |
tnhnrl | 28:16c83a2fdefa | 15 | Modified 2017-11-29 revC by Troy |
tnhnrl | 28:16c83a2fdefa | 16 | - removed depthTolerance variable that was not being used. |
tnhnrl | 28:16c83a2fdefa | 17 | Modified 2017-11-30 revA by Troy/Dan |
tnhnrl | 28:16c83a2fdefa | 18 | - Modified state RISE to setCommand to -1.0 feet (force out of water) and go to next state (float level) when above 0.5 feet |
tnhnrl | 28:16c83a2fdefa | 19 | - Modified sub-state NEUTRAL_SINKING to only move 2.5 mm per cycle instead of 5 mm |
tnhnrl | 28:16c83a2fdefa | 20 | - Commented out home function (accidentally hit home and got stuck while at LASR) |
tnhnrl | 28:16c83a2fdefa | 21 | - Added max depth neutral and dive variables |
tnhnrl | 28:16c83a2fdefa | 22 | - Print max recorded depths and current neutral buoyancy positions |
tnhnrl | 28:16c83a2fdefa | 23 | Modified 2017-11-30 revB by Troy/Dan |
tnhnrl | 28:16c83a2fdefa | 24 | - Dive and Multi-Dive cycles now exit to FLOAT_BROADCAST state |
tnhnrl | 28:16c83a2fdefa | 25 | - FLOAT_LEVEL pitchTolerance variable increased from 1.0 degree to 5.0 degrees (PV was difficult to level in LASR pool) |
tnhnrl | 28:16c83a2fdefa | 26 | - Added the ability to record the states of the FSM and print to user using keyboard "Z" |
tnhnrl | 28:16c83a2fdefa | 27 | Modified 2017-12-01 revA by Troy |
tnhnrl | 28:16c83a2fdefa | 28 | - Minor update to 2017-11-30 revB |
tnhnrl | 28:16c83a2fdefa | 29 | - Now printing current neutral battery and BCE setpoints and max depth in neutral and dive cycle with "C" (and current sensor data) |
tnhnrl | 28:16c83a2fdefa | 30 | - Pool-tested with successful dives. |
tnhnrl | 28:16c83a2fdefa | 31 | 1) Dive working correctly. (And max dive tracker shows that it is reaching the correct commanded depth.) |
tnhnrl | 28:16c83a2fdefa | 32 | 2) Float broadcast working correctly. (And we are now going directly from dive and rise to float broadcast.) |
tnhnrl | 28:16c83a2fdefa | 33 | 3) Neutral sequence has PV diving with -10 to -20 degrees of pitch, battery command seems to be slow or inactive. |
tnhnrl | 28:16c83a2fdefa | 34 | 3) Float broadcast working correctly. |
tnhnrl | 28:16c83a2fdefa | 35 | 4) FSM tracker and sub-FSM tracker working. |
tnhnrl | 28:16c83a2fdefa | 36 | 5) Antenna working until tail just under water (last depth recorded varies between 1.0 and 1.5 feet) |
danstrider | 10:085ab7328054 | 37 | */ |
tnhnrl | 20:8987a9ae2bc7 | 38 | |
tzyoung | 0:ea293bbf9717 | 39 | #include "mbed.h" |
tzyoung | 0:ea293bbf9717 | 40 | #include "StaticDefs.hpp" |
tnhnrl | 20:8987a9ae2bc7 | 41 | |
tnhnrl | 20:8987a9ae2bc7 | 42 | // loop rate timer for slowing down how fast while(1) runs in main() |
tnhnrl | 20:8987a9ae2bc7 | 43 | Ticker loop_rate_ticker; |
tnhnrl | 20:8987a9ae2bc7 | 44 | volatile bool loop = false; |
tnhnrl | 20:8987a9ae2bc7 | 45 | void loop_trigger() { loop = true; }; // loop trigger |
tnhnrl | 20:8987a9ae2bc7 | 46 | |
danstrider | 10:085ab7328054 | 47 | void setup() { |
danstrider | 11:3b241ecb75ed | 48 | pc().baud(57600); |
tnhnrl | 28:16c83a2fdefa | 49 | pc().printf("\n\n\rFSG POOL TEST 2017-12-01 revA\n\n\r"); |
tnhnrl | 20:8987a9ae2bc7 | 50 | |
danstrider | 10:085ab7328054 | 51 | // start up the system timer |
tzyoung | 0:ea293bbf9717 | 52 | systemTime().start(); |
tnhnrl | 20:8987a9ae2bc7 | 53 | |
danstrider | 10:085ab7328054 | 54 | // set up and start the adc. This runs on a fixed interval and is interrupt driven |
tzyoung | 0:ea293bbf9717 | 55 | adc().initialize(); |
tzyoung | 0:ea293bbf9717 | 56 | adc().start(); |
danstrider | 10:085ab7328054 | 57 | |
danstrider | 10:085ab7328054 | 58 | // set up and start the imu. This polls in the background |
danstrider | 10:085ab7328054 | 59 | imu().initialize(); |
danstrider | 10:085ab7328054 | 60 | imu().start(); |
danstrider | 10:085ab7328054 | 61 | |
danstrider | 10:085ab7328054 | 62 | // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298 |
danstrider | 14:85b64a4d08e8 | 63 | depth().init(); |
danstrider | 14:85b64a4d08e8 | 64 | depth().tare(); |
danstrider | 10:085ab7328054 | 65 | |
danstrider | 10:085ab7328054 | 66 | // construct a local file system |
danstrider | 10:085ab7328054 | 67 | local(); |
tnhnrl | 20:8987a9ae2bc7 | 68 | |
danstrider | 10:085ab7328054 | 69 | // load config data from files |
tnhnrl | 21:38c8544db6f4 | 70 | configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt" |
tnhnrl | 21:38c8544db6f4 | 71 | configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt" |
tnhnrl | 21:38c8544db6f4 | 72 | configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position) |
tnhnrl | 21:38c8544db6f4 | 73 | configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position) |
tnhnrl | 20:8987a9ae2bc7 | 74 | |
danstrider | 10:085ab7328054 | 75 | // set up the linear actuators. adc has to be running first. |
danstrider | 10:085ab7328054 | 76 | bce().init(); |
danstrider | 10:085ab7328054 | 77 | bce().start(); |
mkelly10 | 12:a0519d11d2b6 | 78 | bce().pause(); // start by not moving |
tnhnrl | 20:8987a9ae2bc7 | 79 | |
danstrider | 10:085ab7328054 | 80 | batt().init(); |
tzyoung | 2:892b58e56712 | 81 | batt().start(); |
mkelly10 | 12:a0519d11d2b6 | 82 | batt().pause(); // start by not moving |
tnhnrl | 20:8987a9ae2bc7 | 83 | |
danstrider | 10:085ab7328054 | 84 | // set up the depth and pitch outer loop controllers |
danstrider | 10:085ab7328054 | 85 | depthLoop().init(); |
tnhnrl | 13:84fcbe1dcd62 | 86 | depthLoop().start(); |
tnhnrl | 16:3363b9f14913 | 87 | depthLoop().setCommand(stateMachine().getDepthCommand()); |
tnhnrl | 20:8987a9ae2bc7 | 88 | |
danstrider | 10:085ab7328054 | 89 | pitchLoop().init(); |
tnhnrl | 13:84fcbe1dcd62 | 90 | pitchLoop().start(); |
tnhnrl | 16:3363b9f14913 | 91 | pitchLoop().setCommand(stateMachine().getPitchCommand()); |
tnhnrl | 20:8987a9ae2bc7 | 92 | |
tnhnrl | 20:8987a9ae2bc7 | 93 | // show that the PID gains are loading from the file |
tnhnrl | 21:38c8544db6f4 | 94 | 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 | 95 | 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 | 96 | 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 | 97 | 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 | 98 | pc().printf("\n\r"); |
tnhnrl | 21:38c8544db6f4 | 99 | |
tnhnrl | 17:7c16b5671d0e | 100 | //load sequence from file |
tnhnrl | 17:7c16b5671d0e | 101 | sequenceController().loadSequence(); |
tnhnrl | 20:8987a9ae2bc7 | 102 | |
tnhnrl | 20:8987a9ae2bc7 | 103 | // establish the main loop rate |
tnhnrl | 20:8987a9ae2bc7 | 104 | loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate |
tnhnrl | 21:38c8544db6f4 | 105 | } |
tnhnrl | 20:8987a9ae2bc7 | 106 | |
danstrider | 10:085ab7328054 | 107 | int main() { |
danstrider | 10:085ab7328054 | 108 | setup(); |
tnhnrl | 16:3363b9f14913 | 109 | |
danstrider | 10:085ab7328054 | 110 | while(1) { |
tnhnrl | 20:8987a9ae2bc7 | 111 | // does this stuff at the loop rate |
tnhnrl | 20:8987a9ae2bc7 | 112 | if(loop) { |
tnhnrl | 20:8987a9ae2bc7 | 113 | led1() = !led1(); // blink |
tnhnrl | 20:8987a9ae2bc7 | 114 | stateMachine().runStateMachine(); |
tnhnrl | 20:8987a9ae2bc7 | 115 | loop = false; // wait until the loop rate timer fires again |
tnhnrl | 17:7c16b5671d0e | 116 | } |
danstrider | 10:085ab7328054 | 117 | } |
danstrider | 10:085ab7328054 | 118 | } |