update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
main.cpp
- Committer:
- danstrider
- Date:
- 2017-10-23
- Revision:
- 10:085ab7328054
- Parent:
- 5:15bd96205bb2
- Child:
- 11:3b241ecb75ed
File content as of revision 10:085ab7328054:
/* Starting from Trent's Linear Actuator code from 2017-10-19, these modifications by Dan add an outer loop controller for depth and pitch to command the inner linear actuator loops. Modified 2017-10-20 revA by Dan - added outer loop controller, but it is hanging the mbed. (turns out it was the imu update) Modified 2017-10-22 revA by Dan - outer loop now works with a call to outerloop.update() in main loop(), not with an attached ticker Modified 2017-10-22 revB by Dan - enabled both depth and pitch outer loop controllers - added ability to keyboard reset Modified 2017-10-22 revC by Dan - major update to the IMU library processing to make a state machine that doesn't hang - added lat/lon/alt and GNSS fix information to the IMU library - brought out the pin names into the constructors of IMU, omega, SpiADC Modified 2017-10-22 revD by Dan - everything seems to be working, shy of re-checking on the hardware - Depth sensor call done inside the OuterLoop, but should somehow be set as a callback instead - IMU sensor call done inside the OuterLoop, but should somehow be set as a callback instead Modified 2017-10-23 revA by Dan/Matt - linear actuator hardware works great, limit switches, sensing, etc. - outer loops run, but move the BCE in the wrong direction. - new IMU code doesn't read from the sensor correctly, but doesn't hang up either. - depth sensor worked fine, just needs zero bias adjustment. */ #include "mbed.h" #include "StaticDefs.hpp" #include "config_functions.h" extern "C" void mbed_reset(); // utilized to reset the mbed void setup() { pc().printf("\n\n\r\rFSG 2017-10-22 revD\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().initialize(); // construct a local file system local(); // load config data from files load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt" load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt" load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" // set up the linear actuators. adc has to be running first. bce().init(); bce().start(); bce().setPosition_mm(bce().getPosition_mm()); // start by not moving batt().init(); batt().start(); batt().setPosition_mm(batt().getPosition_mm()); // start by not moving // set up the depth and pitch outer loop controllers depthLoop().init(); depthLoop().setCommand(0.0); pitchLoop().init(); pitchLoop().setCommand(0.0); // do not leave this in. Check that PID gains are loading pc().printf("bce P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f \n\r", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope()); pc().printf("batt P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f \n\r", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope()); pc().printf("depth P: %3.2f, I: %3.2f, D %3.2f, \n\r", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD()); pc().printf("pitch P: %3.2f, I: %3.2f, D %3.2f, \n\r", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD()); pc().printf("\n\r"); } void keyboard() { static float setpoint = 50.0; static bool isOuterLoopActive = false; // allow user to select if the outer loop is active char userInput; // check keyboard and make settings changes as requested if (pc().readable()) { // get the key userInput = pc().getc(); // check command against desired control buttons if (userInput == '+' or userInput == '=') { setpoint += 1.0; //increment the command } else if (userInput == '-' or userInput == '_') { setpoint -= 1.0; //decrement the command } else if (userInput == 't' or userInput == 'T') { isOuterLoopActive = !isOuterLoopActive; // toggle the outer loop if (isOuterLoopActive) pc().printf("Outer loops are now active \n\r"); else pc().printf("Outer loops are now inactive \n\r"); } else if (userInput == '\r') { if (isOuterLoopActive) { pc().printf("setting outer loop: %3.0f \n\r", setpoint); depthLoop().setCommand(setpoint); bce().setPosition_mm(depthLoop().getOutput()); // connect outer to inner loop pitchLoop().setCommand(setpoint); batt().setPosition_mm(pitchLoop().getOutput()); // connect outer to inner loop } else { pc().printf("setting inner loop: %3.0f \n\r", setpoint); batt().setPosition_mm(setpoint); bce().setPosition_mm(setpoint); } } else if (userInput == '?') { pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n"); mbed_reset(); } } if (isOuterLoopActive) pc().printf("setpoint %3.1f, depth: %3.1f ft, bce: %3.1f mm \r", setpoint, depth().getDepth(), bce().getPosition_mm()); //pc().printf("setpoint %3.1f, pitch: %3.1f deg, batt: %3.1f mm \r", setpoint, imu().getPitch(), batt().getPosition_mm()); else pc().printf("setpoint %3.1f, bce: %3.1f mm \r", setpoint, bce().getPosition_mm()); //pc().printf("setpoint %3.1f, batt: %3.1f mm \r", setpoint, batt().getPosition_mm()); } void loop() { led1() = !led1(); // blink keyboard(); // sequence controllers... // check whether depth has been triggered // if so, move on to the next line of the script // if done, surface or repeat } int main() { setup(); while(1) { loop(); } }