update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
main.cpp
- Committer:
- tnhnrl
- Date:
- 2017-12-06
- Revision:
- 31:8616e397c22d
- Parent:
- 30:2964617e7676
- Child:
- 32:f2f8ae34aadc
File content as of revision 31:8616e397c22d:
/* Modified 2017-11-29 revA by Troy - changelog.txt carries previous changes - Incorporated email changes and did quick bench test 1) In FIND_NEUTRAL, add bce().setPosition_mm(bceFloatPosition) to the one-shot actions after the unpause(). 2) NEUTRAL_FIRST_PITCH, changed batt position by half from 1.0 to 0.5. 3) NEUTRAL_SINKING and NEUTRAL_SLOWLY_RISE are using getSetPosition now 4) previousPosition_mm deleted (and print statements changed) 5) NEUTRAL_SLOWLY_RISE check is using bce().getSetPosition_mm() 6) Timers replaced with _neutral_timer variable 7) Default timeout is 8 minutes = 480 seconds - Redid the sub-state recording method to only update on state changes Modified 2017-11-29 revB by Troy - minor print fixes Modified 2017-11-29 revC by Troy - removed depthTolerance variable that was not being used. Modified 2017-11-30 revA by Troy/Dan - 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 - Modified sub-state NEUTRAL_SINKING to only move 2.5 mm per cycle instead of 5 mm - Commented out home function (accidentally hit home and got stuck while at LASR) - Added max depth neutral and dive variables - Print max recorded depths and current neutral buoyancy positions Modified 2017-11-30 revB by Troy/Dan - Dive and Multi-Dive cycles now exit to FLOAT_BROADCAST state - FLOAT_LEVEL pitchTolerance variable increased from 1.0 degree to 5.0 degrees (PV was difficult to level in LASR pool) - Added the ability to record the states of the FSM and print to user using keyboard "Z" Modified 2017-12-01 revA by Troy - Minor update to 2017-11-30 revB - Now printing current neutral battery and BCE setpoints and max depth in neutral and dive cycle with "C" (and current sensor data) - Pool-tested with successful dives. 1) Dive working correctly. (And max dive tracker shows that it is reaching the correct commanded depth.) 2) Float broadcast working correctly. (And we are now going directly from dive and rise to float broadcast.) 3) Neutral sequence has PV diving with -10 to -20 degrees of pitch, battery command seems to be slow or inactive. 3) Float broadcast working correctly. 4) FSM tracker and sub-FSM tracker working. 5) Antenna working until tail just under water (last depth recorded varies between 1.0 and 1.5 feet) Modified 2017-12-06 revA by Troy - Added "_isTimeoutRunning = false;" to the FIND_NEUTRAL successful exit case ("else if (runNeutralStateMachine() == NEUTRAL_EXIT)" 1) This will reset the timeout when the PV has successfully found and saved the neutral BCE and battery positions - Successful dive and multi-dive sequences exit to FLOAT_BROADCAST state (fix was implemented in code that did not respond, reimplemented here) 1) Also made the change to the SequenceController (which loads a file that has multiple sequences) - FIND_NEUTRAL starts in NEUTRAL_SINKING with fix on sub-FSM to start with NEUTRAL_SINKING (verified via mbed on bench) */ #include "mbed.h" #include "StaticDefs.hpp" // loop rate timer for slowing down how fast while(1) runs in main() Ticker loop_rate_ticker; volatile bool loop = false; void loop_trigger() { loop = true; }; // loop trigger void setup() { pc().baud(57600); pc().printf("\n\n\rFSG POOL TEST 2017-12-01 revA\n\n\r"); // start up the system timer systemTime().start(); // set up and start the adc. This runs on a fixed interval and is interrupt driven adc().initialize(); adc().start(); // set up and start the imu. This polls in the background imu().initialize(); imu().start(); // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298 depth().init(); depth().tare(); // construct a local file system local(); // load config data from files configFileIO().load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt" configFileIO().load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt" configFileIO().load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" (contains neutral position) configFileIO().load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" (contains neutral position) // set up the linear actuators. adc has to be running first. bce().init(); bce().start(); bce().pause(); // start by not moving batt().init(); batt().start(); batt().pause(); // start by not moving // set up the depth and pitch outer loop controllers depthLoop().init(); depthLoop().start(); depthLoop().setCommand(stateMachine().getDepthCommand()); pitchLoop().init(); pitchLoop().start(); pitchLoop().setCommand(stateMachine().getPitchCommand()); // show that the PID gains are loading from the file 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()); 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()); 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()); 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()); pc().printf("\n\r"); //load sequence from file sequenceController().loadSequence(); // establish the main loop rate loop_rate_ticker.attach(&loop_trigger, 0.1); // fires the ticker at 10 Hz rate } int main() { setup(); while(1) { // does this stuff at the loop rate if(loop) { led1() = !led1(); // blink stateMachine().runStateMachine(); loop = false; // wait until the loop rate timer fires again } } }