update with altimeter, swimfile.txt endleg.txt, etc see changes_13sep.txt also reset_PI()
Dependencies: mbed MODSERIAL FATFileSystem
Diff: main.cpp
- Revision:
- 10:085ab7328054
- Parent:
- 5:15bd96205bb2
- Child:
- 11:3b241ecb75ed
--- a/main.cpp Fri Oct 20 11:41:22 2017 +0000 +++ b/main.cpp Mon Oct 23 12:50:53 2017 +0000 @@ -1,120 +1,155 @@ +/* + 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" -int main() -{ - float positionCmd = 10.0; - char userInput; - - local(); //this makes sure the local file system is constructed - - //Read in and load the BCE parameters from the text file "bce.txt" - load_BCE_config(); // should say "bce.load_config(bce.txt); - bce().init(); +extern "C" void mbed_reset(); // utilized to reset the mbed - ////Read in and load the battery mover parameters from the text file "batt.txt" - load_BATT_config(); - batt().init(); +void setup() { + pc().printf("\n\n\r\rFSG 2017-10-22 revD\n\r"); - // do not leave this in. It is just there to check that things are working - pc().printf("\n\rbce P: %3.2f I: %3.2f D %3.2f zero: %3i limit %3.2f slope %3.2f \n\r", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope()); - pc().printf("\n\rbatt P: %3.2f I: %3.2f D %3.2f zero: %3i limit %3.2f slope %3.2f \n\r", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope()); - //Front load the desired parameters into the linear acuator objects. - //This could be done using ConfigFile, if it worked - - //I need to also check up in whether the limits are being passed to the linear - //actuator's PID objects. I noticed I have defaults that are appropriate for only - //the bouyancy engine - - //start up the system timer + // start up the system timer systemTime().start(); - //setup and start the adc. This runs on a fixed interval and is interrupt driven + // 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(); - //start the bce and batt control loops. ADC has to be running first. - //The motors will not activate until their respective position filters have - //converged, but just to be sure we pause them until we want them to really run - bce().start(); - bce().pause(); - - bce().setPosition_mm(positionCmd); + // 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" - bce().unpause(); + // 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().pause(); - - batt().setPosition_mm(positionCmd); - - batt().unpause(); + 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"); +} - - - while(1) { - - - if (pc().readable()) { - // get the key - userInput = pc().getc(); +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 == '=') { - //increment the duty cycle - positionCmd += 5.0 ; - } else if (userInput == '-') { - //decrement the duty cycle - positionCmd -= 5.0 ; - } else if (userInput == '\r') { - batt().setPosition_mm(positionCmd); - bce().setPosition_mm(positionCmd); + // 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); } } - //pc().printf("set point %3.1f \r", positionCmd); - //pc().printf("pos: %3.2f mm pos: %3.2f counts vel: % 3.2f mm/s Set Point %3.1f \r", bce().getPosition_mm(), bce().getPosition_counts(), bce().getVelocity_mms(), positionCmd); - //pc().printf("pos: %3.2f mm pos: %3.2f counts vel: % 3.2f mm/s Set Point %3.1f \r", batt().getPosition_mm(), batt().getPosition_counts(), batt().getVelocity_mms(), positionCmd); - pc().printf("pos bce: %3.2f mm, pos batt: %3.2f mm, Set Point %3.1f \r", bce().getPosition_mm(), batt().getPosition_mm(), positionCmd); + 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()); } - - - //read in a script file describing the dive cycle - //I envision entries such as - // target pitch angle target depth target depth rate - // 10 degrees 5 ft 0.05 ft/s example dive request - // -10 degrees 0 ft -0.05 ft/s example surface request - - //this implies two pid controllers - // one that manages the batt mover for pitch - // the other manages the buoyance engine for dive speed - - // then some logic is needed to check the box when the desired condition is reached - // like a waypoint threshold. This allows you to get away from worrying as much about - // keeping time +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 +} - //psuedo code to outline what we want to do - /* - check for and parse IMU data - - poll for depth adc reading (eventually this will come from the external adc) - - run depth data through position velocity filter - - update the PID controllers for Pitch and depth rate - - check whether depth has been triggered - if so, move on to the next line of the script - if done , surface or repeat - - */ - - - //This can be ignored for now this was the old serial comms stuff for when I - //was prototyping the BCE controls \ No newline at end of file +int main() { + setup(); + while(1) { + loop(); + } +} \ No newline at end of file