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@13:84fcbe1dcd62, 2017-10-30 (annotated)
- Committer:
- tnhnrl
- Date:
- Mon Oct 30 21:09:19 2017 +0000
- Revision:
- 13:84fcbe1dcd62
- Parent:
- 12:a0519d11d2b6
- Child:
- 14:85b64a4d08e8
Tested hardware for bce and made several pause fixes. Homing works. Added offset to pitch and depth.txt files, and the control loop works. PID gain for depth had to be negative. Next is to replace the motor and test pitch loop.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
danstrider | 10:085ab7328054 | 1 | /* |
danstrider | 10:085ab7328054 | 2 | Starting from Trent's Linear Actuator code from 2017-10-19, these modifications |
danstrider | 10:085ab7328054 | 3 | by Dan add an outer loop controller for depth and pitch to command the inner |
danstrider | 10:085ab7328054 | 4 | linear actuator loops. |
danstrider | 10:085ab7328054 | 5 | Modified 2017-10-20 revA by Dan |
danstrider | 10:085ab7328054 | 6 | - added outer loop controller, but it is hanging the mbed. (turns out it was the imu update) |
danstrider | 10:085ab7328054 | 7 | Modified 2017-10-22 revA by Dan |
danstrider | 10:085ab7328054 | 8 | - outer loop now works with a call to outerloop.update() in main loop(), not with an attached ticker |
danstrider | 10:085ab7328054 | 9 | Modified 2017-10-22 revB by Dan |
danstrider | 10:085ab7328054 | 10 | - enabled both depth and pitch outer loop controllers |
danstrider | 10:085ab7328054 | 11 | - added ability to keyboard reset |
danstrider | 10:085ab7328054 | 12 | Modified 2017-10-22 revC by Dan |
danstrider | 10:085ab7328054 | 13 | - major update to the IMU library processing to make a state machine that doesn't hang |
danstrider | 10:085ab7328054 | 14 | - added lat/lon/alt and GNSS fix information to the IMU library |
danstrider | 10:085ab7328054 | 15 | - brought out the pin names into the constructors of IMU, omega, SpiADC |
danstrider | 10:085ab7328054 | 16 | Modified 2017-10-22 revD by Dan |
danstrider | 10:085ab7328054 | 17 | - everything seems to be working, shy of re-checking on the hardware |
danstrider | 10:085ab7328054 | 18 | - Depth sensor call done inside the OuterLoop, but should somehow be set as a callback instead |
danstrider | 10:085ab7328054 | 19 | - IMU sensor call done inside the OuterLoop, but should somehow be set as a callback instead |
danstrider | 10:085ab7328054 | 20 | Modified 2017-10-23 revA by Dan/Matt |
danstrider | 10:085ab7328054 | 21 | - linear actuator hardware works great, limit switches, sensing, etc. |
danstrider | 10:085ab7328054 | 22 | - outer loops run, but move the BCE in the wrong direction. |
danstrider | 10:085ab7328054 | 23 | - new IMU code doesn't read from the sensor correctly, but doesn't hang up either. |
danstrider | 10:085ab7328054 | 24 | - depth sensor worked fine, just needs zero bias adjustment. |
danstrider | 11:3b241ecb75ed | 25 | Modified 2017-10-24 by Troy |
danstrider | 11:3b241ecb75ed | 26 | - added offset to outerloop |
danstrider | 11:3b241ecb75ed | 27 | Modified 2017-10-26 by Dan |
danstrider | 11:3b241ecb75ed | 28 | - brought over a state machine and new keyboard controls ... currently just dumped into main. |
danstrider | 11:3b241ecb75ed | 29 | Modified 2017-10-26 by Matt |
danstrider | 11:3b241ecb75ed | 30 | - new IMU code imported and working well with the hardware. |
danstrider | 11:3b241ecb75ed | 31 | Modified 2017-10-26 revB by Dan |
danstrider | 11:3b241ecb75ed | 32 | - This version has been in the pool. |
danstrider | 11:3b241ecb75ed | 33 | - Get occasional ADC bad string pot batt & piston. On initial sensor reads, see negative positions. |
danstrider | 11:3b241ecb75ed | 34 | But after running FLOAT_BROADCAST, string pot positions are normal. Not sure why. |
danstrider | 11:3b241ecb75ed | 35 | - Repeatedly got stuck in RISE with a +60s timeout. Battery hit end bell and stalled out. |
danstrider | 11:3b241ecb75ed | 36 | - keyboard and state machine are in main, probably shouldn't be, but easier to debug. |
danstrider | 11:3b241ecb75ed | 37 | - Really happy with the logic and flow of the state machine. Timeouts work. |
danstrider | 11:3b241ecb75ed | 38 | - Need to add a means to drive the linear actuators manually ... toggle out stop() in SIT_IDLE. |
danstrider | 11:3b241ecb75ed | 39 | - Need to add keyboard commands to modify the zeroOffset positions. |
danstrider | 10:085ab7328054 | 40 | */ |
danstrider | 10:085ab7328054 | 41 | |
tzyoung | 0:ea293bbf9717 | 42 | #include "mbed.h" |
tzyoung | 0:ea293bbf9717 | 43 | #include "StaticDefs.hpp" |
tzyoung | 2:892b58e56712 | 44 | #include "config_functions.h" |
tzyoung | 2:892b58e56712 | 45 | |
danstrider | 11:3b241ecb75ed | 46 | |
danstrider | 10:085ab7328054 | 47 | extern "C" void mbed_reset(); // utilized to reset the mbed |
tzyoung | 4:66f13fbb035d | 48 | |
danstrider | 11:3b241ecb75ed | 49 | // state enumerations |
danstrider | 11:3b241ecb75ed | 50 | enum { |
danstrider | 11:3b241ecb75ed | 51 | SIT_IDLE, // stops both motors, exits after a keyboard input |
danstrider | 11:3b241ecb75ed | 52 | KEYBOARD, // handles an individual keypress, exits to state by a keyboard menu |
danstrider | 11:3b241ecb75ed | 53 | FIND_NEUTRAL, // dives to depth at zero pitch, exits when stable |
danstrider | 11:3b241ecb75ed | 54 | DIVE, // dives to depth at negative pitch, exits when crossing a defined depth |
danstrider | 11:3b241ecb75ed | 55 | RISE, // rises to surface at positive pitch, exits when near surface |
danstrider | 11:3b241ecb75ed | 56 | FLOAT_LEVEL, // bce position to float, pitch loop active at zero, exits when stable near zero pitch |
danstrider | 11:3b241ecb75ed | 57 | FLOAT_BROADCAST, // bce position to float, batt position forward to hold tail up, exits when actuators done |
danstrider | 11:3b241ecb75ed | 58 | EMERGENCY_CLIMB // bce position to full rise, batt position to full aft, exits when at surface |
danstrider | 11:3b241ecb75ed | 59 | }; |
danstrider | 11:3b241ecb75ed | 60 | |
danstrider | 11:3b241ecb75ed | 61 | // timer for the state machine |
danstrider | 11:3b241ecb75ed | 62 | Timer timer; |
danstrider | 11:3b241ecb75ed | 63 | |
danstrider | 11:3b241ecb75ed | 64 | // these are all the parameters needed for the state machine |
danstrider | 11:3b241ecb75ed | 65 | int timeout = 60; // generic timeout for every state, seconds |
danstrider | 11:3b241ecb75ed | 66 | float depthTolerance = 0.25; // depth tolerance for neutral finding exit critera |
danstrider | 11:3b241ecb75ed | 67 | float pitchTolerance = 1.0; // pitch angle tolerance for neutral finding exit criteria |
danstrider | 11:3b241ecb75ed | 68 | float bceFloatPosition = 300; // bce position for "float" states |
danstrider | 11:3b241ecb75ed | 69 | float battFloatPosition = 50; // batt position for "broadcast" state |
danstrider | 11:3b241ecb75ed | 70 | float depthCommand = 3.5; // user keyboard depth |
danstrider | 11:3b241ecb75ed | 71 | float pitchCommand = -5.0; // user keyboard depth |
danstrider | 11:3b241ecb75ed | 72 | |
danstrider | 11:3b241ecb75ed | 73 | |
danstrider | 10:085ab7328054 | 74 | void setup() { |
danstrider | 11:3b241ecb75ed | 75 | pc().printf("\n\n\r\rFSG 2017-10-26 revA\n\r"); |
danstrider | 11:3b241ecb75ed | 76 | pc().baud(57600); |
tzyoung | 4:66f13fbb035d | 77 | |
danstrider | 10:085ab7328054 | 78 | // start up the system timer |
tzyoung | 0:ea293bbf9717 | 79 | systemTime().start(); |
tzyoung | 0:ea293bbf9717 | 80 | |
danstrider | 10:085ab7328054 | 81 | // set up and start the adc. This runs on a fixed interval and is interrupt driven |
tzyoung | 0:ea293bbf9717 | 82 | adc().initialize(); |
tzyoung | 0:ea293bbf9717 | 83 | adc().start(); |
danstrider | 10:085ab7328054 | 84 | |
danstrider | 10:085ab7328054 | 85 | // set up and start the imu. This polls in the background |
danstrider | 10:085ab7328054 | 86 | imu().initialize(); |
danstrider | 10:085ab7328054 | 87 | imu().start(); |
danstrider | 10:085ab7328054 | 88 | |
danstrider | 10:085ab7328054 | 89 | // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298 |
danstrider | 10:085ab7328054 | 90 | depth().initialize(); |
danstrider | 10:085ab7328054 | 91 | |
danstrider | 10:085ab7328054 | 92 | // construct a local file system |
danstrider | 10:085ab7328054 | 93 | local(); |
tzyoung | 0:ea293bbf9717 | 94 | |
danstrider | 10:085ab7328054 | 95 | // load config data from files |
danstrider | 10:085ab7328054 | 96 | load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt" |
danstrider | 10:085ab7328054 | 97 | load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt" |
danstrider | 10:085ab7328054 | 98 | load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt" |
danstrider | 10:085ab7328054 | 99 | load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt" |
tzyoung | 4:66f13fbb035d | 100 | |
danstrider | 10:085ab7328054 | 101 | // set up the linear actuators. adc has to be running first. |
danstrider | 10:085ab7328054 | 102 | bce().init(); |
danstrider | 10:085ab7328054 | 103 | bce().start(); |
mkelly10 | 12:a0519d11d2b6 | 104 | bce().pause(); // start by not moving |
tzyoung | 4:66f13fbb035d | 105 | |
danstrider | 10:085ab7328054 | 106 | batt().init(); |
tzyoung | 2:892b58e56712 | 107 | batt().start(); |
mkelly10 | 12:a0519d11d2b6 | 108 | batt().pause(); // start by not moving |
danstrider | 10:085ab7328054 | 109 | |
danstrider | 10:085ab7328054 | 110 | // set up the depth and pitch outer loop controllers |
danstrider | 10:085ab7328054 | 111 | depthLoop().init(); |
tnhnrl | 13:84fcbe1dcd62 | 112 | depthLoop().start(); |
danstrider | 11:3b241ecb75ed | 113 | depthLoop().setCommand(-2.5); |
mkelly10 | 5:15bd96205bb2 | 114 | |
danstrider | 10:085ab7328054 | 115 | pitchLoop().init(); |
tnhnrl | 13:84fcbe1dcd62 | 116 | pitchLoop().start(); |
danstrider | 10:085ab7328054 | 117 | pitchLoop().setCommand(0.0); |
danstrider | 10:085ab7328054 | 118 | |
danstrider | 10:085ab7328054 | 119 | // do not leave this in. Check that PID gains are loading |
danstrider | 11:3b241ecb75ed | 120 | pc().printf("bce P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope()); |
danstrider | 11:3b241ecb75ed | 121 | pc().printf("batt P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope()); |
danstrider | 11:3b241ecb75ed | 122 | pc().printf("depth P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset()); |
danstrider | 11:3b241ecb75ed | 123 | pc().printf("pitch P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset()); |
danstrider | 10:085ab7328054 | 124 | pc().printf("\n\r"); |
danstrider | 10:085ab7328054 | 125 | } |
mkelly10 | 5:15bd96205bb2 | 126 | |
danstrider | 11:3b241ecb75ed | 127 | |
danstrider | 11:3b241ecb75ed | 128 | |
danstrider | 11:3b241ecb75ed | 129 | void keyboard_menu_BCE_PID_settings() { |
danstrider | 11:3b241ecb75ed | 130 | char PID_key; |
danstrider | 11:3b241ecb75ed | 131 | float gain_step_size = 0.01; // modify this to change gain step size |
danstrider | 11:3b241ecb75ed | 132 | float KP = bce().getControllerP(); // load current value |
danstrider | 11:3b241ecb75ed | 133 | float KI = bce().getControllerI(); // load current global value |
danstrider | 11:3b241ecb75ed | 134 | float KD = bce().getControllerD(); // load current global value |
danstrider | 11:3b241ecb75ed | 135 | |
danstrider | 11:3b241ecb75ed | 136 | // show the menu |
danstrider | 11:3b241ecb75ed | 137 | pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)"); |
danstrider | 11:3b241ecb75ed | 138 | pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'"); |
danstrider | 11:3b241ecb75ed | 139 | pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r"); |
danstrider | 11:3b241ecb75ed | 140 | pc().printf("bce P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope()); |
danstrider | 11:3b241ecb75ed | 141 | |
danstrider | 11:3b241ecb75ed | 142 | // handle the key presses |
danstrider | 11:3b241ecb75ed | 143 | while(1) { |
danstrider | 11:3b241ecb75ed | 144 | // get the user's keystroke from either of the two inputs |
danstrider | 11:3b241ecb75ed | 145 | if (pc().readable()) { |
danstrider | 11:3b241ecb75ed | 146 | PID_key = pc().getc(); |
danstrider | 11:3b241ecb75ed | 147 | } |
danstrider | 11:3b241ecb75ed | 148 | else { |
danstrider | 11:3b241ecb75ed | 149 | continue; // didn't get a user input, so keep waiting for it |
danstrider | 11:3b241ecb75ed | 150 | } |
danstrider | 11:3b241ecb75ed | 151 | |
danstrider | 11:3b241ecb75ed | 152 | // handle the user's key input |
danstrider | 11:3b241ecb75ed | 153 | if (PID_key == '-') { |
danstrider | 11:3b241ecb75ed | 154 | KP -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 155 | pc().printf("P gain: %0.5f \r\n", KP); |
danstrider | 11:3b241ecb75ed | 156 | } |
danstrider | 11:3b241ecb75ed | 157 | else if (PID_key == '=') { |
danstrider | 11:3b241ecb75ed | 158 | KP += gain_step_size; |
danstrider | 11:3b241ecb75ed | 159 | pc().printf("P gain: %0.5f \r\n", KP); |
danstrider | 11:3b241ecb75ed | 160 | } |
danstrider | 11:3b241ecb75ed | 161 | else if (PID_key == '[') { |
danstrider | 11:3b241ecb75ed | 162 | KI -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 163 | pc().printf("I gain: %0.5f \r\n", KI); |
danstrider | 11:3b241ecb75ed | 164 | } |
danstrider | 11:3b241ecb75ed | 165 | else if (PID_key == ']') { |
danstrider | 11:3b241ecb75ed | 166 | KI += gain_step_size; |
danstrider | 11:3b241ecb75ed | 167 | pc().printf("I gain: %0.5f \r\n", KI); |
danstrider | 11:3b241ecb75ed | 168 | } |
danstrider | 11:3b241ecb75ed | 169 | else if (PID_key == ';') { |
danstrider | 11:3b241ecb75ed | 170 | KD -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 171 | pc().printf("D gain: %0.5f \r\n", KD); |
danstrider | 11:3b241ecb75ed | 172 | } |
danstrider | 11:3b241ecb75ed | 173 | else if (PID_key == '\'') { |
danstrider | 11:3b241ecb75ed | 174 | KD += gain_step_size; |
danstrider | 11:3b241ecb75ed | 175 | pc().printf("D gain: %0.5f \r\n", KD); |
danstrider | 11:3b241ecb75ed | 176 | } |
danstrider | 11:3b241ecb75ed | 177 | else if (PID_key == 'S') { // user wants to save these modified values |
danstrider | 11:3b241ecb75ed | 178 | // set values |
danstrider | 11:3b241ecb75ed | 179 | bce().setControllerP(KP); |
danstrider | 11:3b241ecb75ed | 180 | bce().setControllerI(KI); |
danstrider | 11:3b241ecb75ed | 181 | bce().setControllerD(KD); |
danstrider | 11:3b241ecb75ed | 182 | |
danstrider | 11:3b241ecb75ed | 183 | // save into "PID.cfg" |
danstrider | 11:3b241ecb75ed | 184 | //Config_File_IO().write_manual_position_PID_values_to_config(batt_position_P,batt_position_I,batt_position_D,bce_position_P,bce_position_I,bce_position_D); |
danstrider | 11:3b241ecb75ed | 185 | break; //exit the while loop |
danstrider | 11:3b241ecb75ed | 186 | } |
danstrider | 11:3b241ecb75ed | 187 | else if (PID_key == 'X') { |
danstrider | 11:3b241ecb75ed | 188 | break; //exit the while loop |
danstrider | 11:3b241ecb75ed | 189 | } |
danstrider | 11:3b241ecb75ed | 190 | else { |
danstrider | 11:3b241ecb75ed | 191 | pc().printf("\n\rThis key does nothing here. "); |
danstrider | 11:3b241ecb75ed | 192 | } |
danstrider | 11:3b241ecb75ed | 193 | } |
danstrider | 11:3b241ecb75ed | 194 | } |
danstrider | 11:3b241ecb75ed | 195 | |
danstrider | 11:3b241ecb75ed | 196 | void keyboard_menu_DEPTH_PID_settings() { |
danstrider | 11:3b241ecb75ed | 197 | char PID_key; |
danstrider | 11:3b241ecb75ed | 198 | float gain_step_size = 0.01; // modify this to change gain step size |
danstrider | 11:3b241ecb75ed | 199 | float KP = depthLoop().getControllerP(); // load current global value |
danstrider | 11:3b241ecb75ed | 200 | float KI = depthLoop().getControllerI(); // load current global value |
danstrider | 11:3b241ecb75ed | 201 | float KD = depthLoop().getControllerD(); // load current global value |
danstrider | 11:3b241ecb75ed | 202 | |
danstrider | 11:3b241ecb75ed | 203 | // show the menu |
danstrider | 11:3b241ecb75ed | 204 | pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)"); |
danstrider | 11:3b241ecb75ed | 205 | pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'"); |
danstrider | 11:3b241ecb75ed | 206 | pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\n\n\r"); |
danstrider | 11:3b241ecb75ed | 207 | pc().printf("depth P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset()); |
danstrider | 11:3b241ecb75ed | 208 | |
danstrider | 11:3b241ecb75ed | 209 | // handle the key presses |
danstrider | 11:3b241ecb75ed | 210 | while(1) { |
danstrider | 11:3b241ecb75ed | 211 | // get the user's keystroke from either of the two inputs |
danstrider | 11:3b241ecb75ed | 212 | if (pc().readable()) { |
danstrider | 11:3b241ecb75ed | 213 | PID_key = pc().getc(); |
danstrider | 11:3b241ecb75ed | 214 | } |
danstrider | 11:3b241ecb75ed | 215 | else { |
danstrider | 11:3b241ecb75ed | 216 | continue; // didn't get a user input, so keep waiting for it |
danstrider | 11:3b241ecb75ed | 217 | } |
danstrider | 11:3b241ecb75ed | 218 | |
danstrider | 11:3b241ecb75ed | 219 | // handle the user's key input |
danstrider | 11:3b241ecb75ed | 220 | if (PID_key == '-') { |
danstrider | 11:3b241ecb75ed | 221 | KP -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 222 | pc().printf("P gain: %0.5f \r\n", KP); |
danstrider | 11:3b241ecb75ed | 223 | } |
danstrider | 11:3b241ecb75ed | 224 | else if (PID_key == '=') { |
danstrider | 11:3b241ecb75ed | 225 | KP += gain_step_size; |
danstrider | 11:3b241ecb75ed | 226 | pc().printf("P gain: %0.5f \r\n", KP); |
danstrider | 11:3b241ecb75ed | 227 | } |
danstrider | 11:3b241ecb75ed | 228 | else if (PID_key == '[') { |
danstrider | 11:3b241ecb75ed | 229 | KI -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 230 | pc().printf("I gain: %0.5f \r\n", KI); |
danstrider | 11:3b241ecb75ed | 231 | } |
danstrider | 11:3b241ecb75ed | 232 | else if (PID_key == ']') { |
danstrider | 11:3b241ecb75ed | 233 | KI += gain_step_size; |
danstrider | 11:3b241ecb75ed | 234 | pc().printf("I gain: %0.5f \r\n", KI); |
danstrider | 11:3b241ecb75ed | 235 | } |
danstrider | 11:3b241ecb75ed | 236 | else if (PID_key == ';') { |
danstrider | 11:3b241ecb75ed | 237 | KD -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 238 | pc().printf("D gain: %0.5f \r\n", KD); |
danstrider | 11:3b241ecb75ed | 239 | } |
danstrider | 11:3b241ecb75ed | 240 | else if (PID_key == '\'') { |
danstrider | 11:3b241ecb75ed | 241 | KD += gain_step_size; |
danstrider | 11:3b241ecb75ed | 242 | pc().printf("D gain: %0.5f \r\n", KD); |
danstrider | 11:3b241ecb75ed | 243 | } |
danstrider | 11:3b241ecb75ed | 244 | else if (PID_key == 'S') { // user wants to save these settings |
danstrider | 11:3b241ecb75ed | 245 | // set global values |
danstrider | 11:3b241ecb75ed | 246 | depthLoop().setControllerP(KP); |
danstrider | 11:3b241ecb75ed | 247 | depthLoop().setControllerI(KI); |
danstrider | 11:3b241ecb75ed | 248 | depthLoop().setControllerD(KD); |
danstrider | 11:3b241ecb75ed | 249 | |
danstrider | 11:3b241ecb75ed | 250 | // save into "PID.cfg" |
danstrider | 11:3b241ecb75ed | 251 | //Config_File_IO().write_auto_PID_values_to_config(pitch_controller_P,pitch_controller_I,pitch_controller_D,depth_controller_P,depth_controller_I,depth_controller_D); |
danstrider | 11:3b241ecb75ed | 252 | break; //exit the while loop |
danstrider | 11:3b241ecb75ed | 253 | } |
danstrider | 11:3b241ecb75ed | 254 | else if (PID_key == 'X') { |
danstrider | 11:3b241ecb75ed | 255 | break; //exit the while loop |
danstrider | 11:3b241ecb75ed | 256 | } |
danstrider | 11:3b241ecb75ed | 257 | else { |
danstrider | 11:3b241ecb75ed | 258 | pc().printf("\n\rThis key does nothing here. "); |
danstrider | 11:3b241ecb75ed | 259 | } |
danstrider | 11:3b241ecb75ed | 260 | } |
danstrider | 11:3b241ecb75ed | 261 | } |
danstrider | 11:3b241ecb75ed | 262 | |
danstrider | 11:3b241ecb75ed | 263 | void keyboard_menu_BATT_PID_settings() { |
danstrider | 11:3b241ecb75ed | 264 | char PID_key; |
danstrider | 11:3b241ecb75ed | 265 | float gain_step_size = 0.01; // modify this to change gain step size |
danstrider | 11:3b241ecb75ed | 266 | float KP = batt().getControllerP(); // load current global value |
danstrider | 11:3b241ecb75ed | 267 | float KI = batt().getControllerI(); // load current global value |
danstrider | 11:3b241ecb75ed | 268 | float KD = batt().getControllerD(); // load current global value |
danstrider | 11:3b241ecb75ed | 269 | |
danstrider | 11:3b241ecb75ed | 270 | // print the menu |
danstrider | 11:3b241ecb75ed | 271 | pc().printf("\n\r2: Battery Motor PID gain settings (MENU)"); |
danstrider | 11:3b241ecb75ed | 272 | pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'"); |
danstrider | 11:3b241ecb75ed | 273 | pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r"); |
danstrider | 11:3b241ecb75ed | 274 | pc().printf("batt P: %3.2f, I: %3.2f, D %3.2f, zero %3i, limit %3.0f mm, slope %3.3f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope()); |
danstrider | 11:3b241ecb75ed | 275 | |
danstrider | 11:3b241ecb75ed | 276 | // handle the key presses |
danstrider | 11:3b241ecb75ed | 277 | while(1) { |
danstrider | 11:3b241ecb75ed | 278 | // get the user's keystroke from either of the two inputs |
danstrider | 11:3b241ecb75ed | 279 | if (pc().readable()) { |
danstrider | 11:3b241ecb75ed | 280 | PID_key = pc().getc(); |
danstrider | 11:3b241ecb75ed | 281 | } |
danstrider | 11:3b241ecb75ed | 282 | else { |
danstrider | 11:3b241ecb75ed | 283 | continue; // didn't get a user input, so keep waiting for it |
danstrider | 11:3b241ecb75ed | 284 | } |
danstrider | 11:3b241ecb75ed | 285 | |
danstrider | 11:3b241ecb75ed | 286 | // handle the user's key input |
danstrider | 11:3b241ecb75ed | 287 | if (PID_key == '-') { |
danstrider | 11:3b241ecb75ed | 288 | KP -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 289 | pc().printf("\rP gain: %0.5f ", KP); |
danstrider | 11:3b241ecb75ed | 290 | } |
danstrider | 11:3b241ecb75ed | 291 | else if (PID_key == '=') { |
danstrider | 11:3b241ecb75ed | 292 | KP += gain_step_size; |
danstrider | 11:3b241ecb75ed | 293 | pc().printf("\rP gain: %0.5f ", KP); |
danstrider | 11:3b241ecb75ed | 294 | } |
danstrider | 11:3b241ecb75ed | 295 | else if (PID_key == '[') { |
danstrider | 11:3b241ecb75ed | 296 | KI -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 297 | pc().printf("\rI gain: %0.5f ", KI); |
danstrider | 11:3b241ecb75ed | 298 | } |
danstrider | 11:3b241ecb75ed | 299 | else if (PID_key == ']') { |
danstrider | 11:3b241ecb75ed | 300 | KI += gain_step_size; |
danstrider | 11:3b241ecb75ed | 301 | pc().printf("\rI gain: %0.5f ", KI); |
danstrider | 11:3b241ecb75ed | 302 | } |
danstrider | 11:3b241ecb75ed | 303 | else if (PID_key == ';') { |
danstrider | 11:3b241ecb75ed | 304 | KD -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 305 | pc().printf("\rD gain: %0.5f ", KD); |
danstrider | 11:3b241ecb75ed | 306 | } |
danstrider | 11:3b241ecb75ed | 307 | else if (PID_key == '\'') { |
danstrider | 11:3b241ecb75ed | 308 | KD += gain_step_size; |
danstrider | 11:3b241ecb75ed | 309 | pc().printf("\rD gain: %0.5f ", KD); |
danstrider | 11:3b241ecb75ed | 310 | } |
danstrider | 11:3b241ecb75ed | 311 | else if (PID_key == 'S') { // user wants to save the modified values |
danstrider | 11:3b241ecb75ed | 312 | // set global values |
danstrider | 11:3b241ecb75ed | 313 | batt().setControllerP(KP); |
danstrider | 11:3b241ecb75ed | 314 | batt().setControllerI(KI); |
danstrider | 11:3b241ecb75ed | 315 | batt().setControllerD(KD); |
danstrider | 11:3b241ecb75ed | 316 | |
danstrider | 11:3b241ecb75ed | 317 | // save to "PID.cfg" file |
danstrider | 11:3b241ecb75ed | 318 | //Config_File_IO().write_manual_position_PID_values_to_config(batt_position_P,batt_position_I,batt_position_D,bce_position_P,bce_position_I,bce_position_D); |
danstrider | 11:3b241ecb75ed | 319 | break; //exit the while loop |
danstrider | 11:3b241ecb75ed | 320 | } |
danstrider | 11:3b241ecb75ed | 321 | else if (PID_key == 'X') { |
danstrider | 11:3b241ecb75ed | 322 | break; //exit the while loop |
danstrider | 11:3b241ecb75ed | 323 | } |
danstrider | 11:3b241ecb75ed | 324 | else { |
danstrider | 11:3b241ecb75ed | 325 | pc().printf("This key does nothing here.\r"); |
danstrider | 11:3b241ecb75ed | 326 | } |
danstrider | 11:3b241ecb75ed | 327 | } |
danstrider | 11:3b241ecb75ed | 328 | } |
danstrider | 11:3b241ecb75ed | 329 | |
danstrider | 11:3b241ecb75ed | 330 | void keyboard_menu_PITCH_PID_settings() { |
danstrider | 11:3b241ecb75ed | 331 | char PID_key; |
danstrider | 11:3b241ecb75ed | 332 | float gain_step_size = 0.01; // modify this to change gain step size |
danstrider | 11:3b241ecb75ed | 333 | float KP = pitchLoop().getControllerP(); // load current global value |
danstrider | 11:3b241ecb75ed | 334 | float KI = pitchLoop().getControllerI(); // load current global value |
danstrider | 11:3b241ecb75ed | 335 | float KD = pitchLoop().getControllerD(); // load current global value |
danstrider | 11:3b241ecb75ed | 336 | |
danstrider | 11:3b241ecb75ed | 337 | // print the menu |
danstrider | 11:3b241ecb75ed | 338 | pc().printf("\n\r2: Battery Motor PID gain settings (MENU)"); |
danstrider | 11:3b241ecb75ed | 339 | pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'"); |
danstrider | 11:3b241ecb75ed | 340 | pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r"); |
danstrider | 11:3b241ecb75ed | 341 | pc().printf("pitch P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset()); |
danstrider | 11:3b241ecb75ed | 342 | |
danstrider | 11:3b241ecb75ed | 343 | // handle the key presses |
danstrider | 11:3b241ecb75ed | 344 | while(1) { |
danstrider | 11:3b241ecb75ed | 345 | // get the user's keystroke from either of the two inputs |
danstrider | 11:3b241ecb75ed | 346 | if (pc().readable()) { |
danstrider | 11:3b241ecb75ed | 347 | PID_key = pc().getc(); |
danstrider | 11:3b241ecb75ed | 348 | } |
danstrider | 11:3b241ecb75ed | 349 | else { |
danstrider | 11:3b241ecb75ed | 350 | continue; // didn't get a user input, so keep waiting for it |
danstrider | 11:3b241ecb75ed | 351 | } |
danstrider | 11:3b241ecb75ed | 352 | |
danstrider | 11:3b241ecb75ed | 353 | // handle the user's key input |
danstrider | 11:3b241ecb75ed | 354 | if (PID_key == '-') { |
danstrider | 11:3b241ecb75ed | 355 | KP -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 356 | pc().printf("\rP gain: %0.5f ", KP); |
danstrider | 11:3b241ecb75ed | 357 | } |
danstrider | 11:3b241ecb75ed | 358 | else if (PID_key == '=') { |
danstrider | 11:3b241ecb75ed | 359 | KP += gain_step_size; |
danstrider | 11:3b241ecb75ed | 360 | pc().printf("\rP gain: %0.5f ", KP); |
danstrider | 11:3b241ecb75ed | 361 | } |
danstrider | 11:3b241ecb75ed | 362 | else if (PID_key == '[') { |
danstrider | 11:3b241ecb75ed | 363 | KI -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 364 | pc().printf("\rI gain: %0.5f ", KI); |
danstrider | 11:3b241ecb75ed | 365 | } |
danstrider | 11:3b241ecb75ed | 366 | else if (PID_key == ']') { |
danstrider | 11:3b241ecb75ed | 367 | KI += gain_step_size; |
danstrider | 11:3b241ecb75ed | 368 | pc().printf("\rI gain: %0.5f ", KI); |
danstrider | 11:3b241ecb75ed | 369 | } |
danstrider | 11:3b241ecb75ed | 370 | else if (PID_key == ';') { |
danstrider | 11:3b241ecb75ed | 371 | KD -= gain_step_size; |
danstrider | 11:3b241ecb75ed | 372 | pc().printf("\rD gain: %0.5f ", KD); |
danstrider | 11:3b241ecb75ed | 373 | } |
danstrider | 11:3b241ecb75ed | 374 | else if (PID_key == '\'') { |
danstrider | 11:3b241ecb75ed | 375 | KD += gain_step_size; |
danstrider | 11:3b241ecb75ed | 376 | pc().printf("\rD gain: %0.5f ", KD); |
danstrider | 11:3b241ecb75ed | 377 | } |
danstrider | 11:3b241ecb75ed | 378 | else if (PID_key == 'S') { // user wants to save the modified values |
danstrider | 11:3b241ecb75ed | 379 | // set global values |
danstrider | 11:3b241ecb75ed | 380 | pitchLoop().setControllerP(KP); |
danstrider | 11:3b241ecb75ed | 381 | pitchLoop().setControllerI(KI); |
danstrider | 11:3b241ecb75ed | 382 | pitchLoop().setControllerD(KD); |
danstrider | 11:3b241ecb75ed | 383 | |
danstrider | 11:3b241ecb75ed | 384 | //Config_File_IO().write_auto_PID_values_to_config(pitch_controller_P,pitch_controller_I,pitch_controller_D,depth_controller_P,depth_controller_I,depth_controller_D); |
danstrider | 11:3b241ecb75ed | 385 | break; //exit the while loop |
danstrider | 11:3b241ecb75ed | 386 | } |
danstrider | 11:3b241ecb75ed | 387 | else if (PID_key == 'X') { |
danstrider | 11:3b241ecb75ed | 388 | break; //exit the while loop |
danstrider | 11:3b241ecb75ed | 389 | } |
danstrider | 11:3b241ecb75ed | 390 | else { |
danstrider | 11:3b241ecb75ed | 391 | pc().printf("This key does nothing here.\r"); |
danstrider | 11:3b241ecb75ed | 392 | } |
danstrider | 11:3b241ecb75ed | 393 | } |
danstrider | 11:3b241ecb75ed | 394 | } |
danstrider | 11:3b241ecb75ed | 395 | |
danstrider | 11:3b241ecb75ed | 396 | |
danstrider | 11:3b241ecb75ed | 397 | // output the keyboard menu for user's reference |
danstrider | 11:3b241ecb75ed | 398 | void showMenu() { |
danstrider | 11:3b241ecb75ed | 399 | pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n"); |
danstrider | 11:3b241ecb75ed | 400 | pc().printf(" N to find neutral\r\n"); |
danstrider | 11:3b241ecb75ed | 401 | pc().printf(" D to initiate dive cycle\r\n"); |
danstrider | 11:3b241ecb75ed | 402 | pc().printf(" R to initiate rise\r\n"); |
danstrider | 11:3b241ecb75ed | 403 | pc().printf(" L to float level\r\n"); |
danstrider | 11:3b241ecb75ed | 404 | pc().printf(" B to float at broadcast pitch\r\n"); |
danstrider | 11:3b241ecb75ed | 405 | pc().printf(" E to initiate emergency climb\r\n"); |
mkelly10 | 12:a0519d11d2b6 | 406 | pc().printf(" H to run homing sequence on both BCE and Batt\r\n"); |
danstrider | 11:3b241ecb75ed | 407 | pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",pitchLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 408 | pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",depthLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 409 | pc().printf("+/- to decrease/increase timeout: %d s\r\n",timeout); |
danstrider | 11:3b241ecb75ed | 410 | pc().printf(" 1 BCE PID sub-menu\r\n"); |
danstrider | 11:3b241ecb75ed | 411 | pc().printf(" 2 BATT PID sub-menu\r\n"); |
danstrider | 11:3b241ecb75ed | 412 | pc().printf(" 3 Depth PID sub-menu\r\n"); |
danstrider | 11:3b241ecb75ed | 413 | pc().printf(" 4 Pitch PID sub-menu\r\n"); |
danstrider | 11:3b241ecb75ed | 414 | pc().printf(" C See sensor readings\r\n"); |
danstrider | 11:3b241ecb75ed | 415 | pc().printf(" ? to reset mbed\r\n"); |
danstrider | 11:3b241ecb75ed | 416 | } |
danstrider | 11:3b241ecb75ed | 417 | |
danstrider | 11:3b241ecb75ed | 418 | // keyboard currently handles a key at a time |
danstrider | 11:3b241ecb75ed | 419 | // returns -1 if not a state command |
danstrider | 11:3b241ecb75ed | 420 | // returns a positive number to command a new state |
danstrider | 11:3b241ecb75ed | 421 | int keyboard() { |
danstrider | 10:085ab7328054 | 422 | char userInput; |
danstrider | 10:085ab7328054 | 423 | |
danstrider | 10:085ab7328054 | 424 | // check keyboard and make settings changes as requested |
danstrider | 10:085ab7328054 | 425 | if (pc().readable()) { |
danstrider | 10:085ab7328054 | 426 | // get the key |
danstrider | 10:085ab7328054 | 427 | userInput = pc().getc(); |
mkelly10 | 5:15bd96205bb2 | 428 | |
danstrider | 10:085ab7328054 | 429 | // check command against desired control buttons |
danstrider | 11:3b241ecb75ed | 430 | // change state |
danstrider | 11:3b241ecb75ed | 431 | if (userInput == 'D' or userInput == 'd') { |
danstrider | 11:3b241ecb75ed | 432 | return DIVE; |
danstrider | 10:085ab7328054 | 433 | } |
danstrider | 11:3b241ecb75ed | 434 | else if (userInput == 'N' or userInput == 'n') { |
danstrider | 11:3b241ecb75ed | 435 | return FIND_NEUTRAL; |
danstrider | 11:3b241ecb75ed | 436 | } |
danstrider | 11:3b241ecb75ed | 437 | else if (userInput == 'R' or userInput == 'r') { |
danstrider | 11:3b241ecb75ed | 438 | return RISE; |
danstrider | 11:3b241ecb75ed | 439 | } |
danstrider | 11:3b241ecb75ed | 440 | else if (userInput == 'L' or userInput == 'l') { |
danstrider | 11:3b241ecb75ed | 441 | return FLOAT_LEVEL; |
danstrider | 10:085ab7328054 | 442 | } |
danstrider | 11:3b241ecb75ed | 443 | else if (userInput == 'B' or userInput == 'b') { |
danstrider | 11:3b241ecb75ed | 444 | return FLOAT_BROADCAST; |
danstrider | 11:3b241ecb75ed | 445 | } |
danstrider | 11:3b241ecb75ed | 446 | else if (userInput == 'E' or userInput == 'e') { |
danstrider | 11:3b241ecb75ed | 447 | return EMERGENCY_CLIMB; |
mkelly10 | 5:15bd96205bb2 | 448 | } |
mkelly10 | 12:a0519d11d2b6 | 449 | else if (userInput == 'H' or userInput == 'h') { |
mkelly10 | 12:a0519d11d2b6 | 450 | pc().printf("running homing procedure\r\n"); |
mkelly10 | 12:a0519d11d2b6 | 451 | bce().unpause(); bce().homePiston(); bce().pause(); |
mkelly10 | 12:a0519d11d2b6 | 452 | batt().unpause(); batt().homePiston(); batt().pause(); |
mkelly10 | 12:a0519d11d2b6 | 453 | return SIT_IDLE; |
mkelly10 | 12:a0519d11d2b6 | 454 | } |
danstrider | 10:085ab7328054 | 455 | else if (userInput == '?') { |
danstrider | 10:085ab7328054 | 456 | pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n"); |
danstrider | 11:3b241ecb75ed | 457 | wait(0.5); |
danstrider | 10:085ab7328054 | 458 | mbed_reset(); |
danstrider | 10:085ab7328054 | 459 | } |
danstrider | 11:3b241ecb75ed | 460 | |
danstrider | 11:3b241ecb75ed | 461 | // change settings |
danstrider | 11:3b241ecb75ed | 462 | else if (userInput == 'Q' or userInput == 'q') { |
danstrider | 11:3b241ecb75ed | 463 | pitchCommand -= 0.5; //decrement the pitch setpoint |
danstrider | 11:3b241ecb75ed | 464 | pitchLoop().setCommand(pitchCommand); |
danstrider | 11:3b241ecb75ed | 465 | pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 466 | } |
danstrider | 11:3b241ecb75ed | 467 | else if (userInput == 'W' or userInput == 'w') { |
danstrider | 11:3b241ecb75ed | 468 | pitchCommand += 0.5; //increment the pitch setpoint |
danstrider | 11:3b241ecb75ed | 469 | pitchLoop().setCommand(pitchCommand); |
danstrider | 11:3b241ecb75ed | 470 | pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 471 | } |
danstrider | 11:3b241ecb75ed | 472 | else if (userInput == 'A' or userInput == 'a') { |
danstrider | 11:3b241ecb75ed | 473 | depthCommand -= 0.5; //decrement the depth setpoint |
danstrider | 11:3b241ecb75ed | 474 | depthLoop().setCommand(depthCommand); |
danstrider | 11:3b241ecb75ed | 475 | pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 476 | } |
danstrider | 11:3b241ecb75ed | 477 | else if (userInput == 'S' or userInput == 's') { |
danstrider | 11:3b241ecb75ed | 478 | depthCommand += 0.5; //increment the depth setpoint |
danstrider | 11:3b241ecb75ed | 479 | depthLoop().setCommand(depthCommand); |
danstrider | 11:3b241ecb75ed | 480 | pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 481 | } |
danstrider | 11:3b241ecb75ed | 482 | else if (userInput == '-') { |
danstrider | 11:3b241ecb75ed | 483 | timeout -= 10.0; //decrement the timeout |
danstrider | 11:3b241ecb75ed | 484 | pc().printf(">>> timeout decreased: %d\r\n", timeout); |
danstrider | 11:3b241ecb75ed | 485 | } |
danstrider | 11:3b241ecb75ed | 486 | else if (userInput == '=' or userInput == '+') { |
danstrider | 11:3b241ecb75ed | 487 | timeout += 10.0; //increment the timeout |
danstrider | 11:3b241ecb75ed | 488 | pc().printf(">>> timeout increased: %d\r\n", timeout); |
danstrider | 11:3b241ecb75ed | 489 | } |
danstrider | 11:3b241ecb75ed | 490 | |
danstrider | 11:3b241ecb75ed | 491 | // add keyboard commands to move the neutral zero offsets, both bce and batt |
danstrider | 11:3b241ecb75ed | 492 | |
danstrider | 11:3b241ecb75ed | 493 | // go to sub-menus for the PID gains (this is blocking) |
danstrider | 11:3b241ecb75ed | 494 | else if (userInput == '1') { |
danstrider | 11:3b241ecb75ed | 495 | keyboard_menu_BCE_PID_settings(); |
danstrider | 11:3b241ecb75ed | 496 | } |
danstrider | 11:3b241ecb75ed | 497 | else if (userInput == '2') { |
danstrider | 11:3b241ecb75ed | 498 | keyboard_menu_BATT_PID_settings(); |
danstrider | 11:3b241ecb75ed | 499 | } |
danstrider | 11:3b241ecb75ed | 500 | else if (userInput == '3') { |
danstrider | 11:3b241ecb75ed | 501 | keyboard_menu_DEPTH_PID_settings(); |
danstrider | 11:3b241ecb75ed | 502 | } |
danstrider | 11:3b241ecb75ed | 503 | else if (userInput == '4') { |
danstrider | 11:3b241ecb75ed | 504 | keyboard_menu_PITCH_PID_settings(); |
danstrider | 11:3b241ecb75ed | 505 | } |
danstrider | 11:3b241ecb75ed | 506 | |
danstrider | 11:3b241ecb75ed | 507 | else if (userInput == 'C' or userInput == 'c') { |
tnhnrl | 13:84fcbe1dcd62 | 508 | pc().printf("depth: %3.1f\r\n",depthLoop().getPosition()); |
danstrider | 11:3b241ecb75ed | 509 | pc().printf("pitch: %3.1f\r\n",imu().getPitch()); |
danstrider | 11:3b241ecb75ed | 510 | pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm()); |
danstrider | 11:3b241ecb75ed | 511 | pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm()); |
danstrider | 11:3b241ecb75ed | 512 | pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm()); |
danstrider | 11:3b241ecb75ed | 513 | pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm()); |
danstrider | 11:3b241ecb75ed | 514 | pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 515 | pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 516 | } |
danstrider | 11:3b241ecb75ed | 517 | else { |
danstrider | 11:3b241ecb75ed | 518 | return (-1); |
danstrider | 11:3b241ecb75ed | 519 | } |
mkelly10 | 5:15bd96205bb2 | 520 | } |
danstrider | 11:3b241ecb75ed | 521 | return (-1); |
mkelly10 | 5:15bd96205bb2 | 522 | } |
mkelly10 | 5:15bd96205bb2 | 523 | |
danstrider | 11:3b241ecb75ed | 524 | void stateMachine() { |
danstrider | 11:3b241ecb75ed | 525 | static int state = SIT_IDLE; // select starting state here |
danstrider | 11:3b241ecb75ed | 526 | static bool isTimeoutRunning = false; // default timer to not running |
danstrider | 11:3b241ecb75ed | 527 | |
danstrider | 11:3b241ecb75ed | 528 | // finite state machine ... each state has at least one exit criteria |
danstrider | 11:3b241ecb75ed | 529 | switch (state) { |
danstrider | 11:3b241ecb75ed | 530 | case SIT_IDLE : |
danstrider | 11:3b241ecb75ed | 531 | // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions |
danstrider | 11:3b241ecb75ed | 532 | if (!isTimeoutRunning) { |
danstrider | 11:3b241ecb75ed | 533 | showMenu(); |
danstrider | 11:3b241ecb75ed | 534 | pc().printf("\r\nstate: SIT_IDLE\r\n"); |
danstrider | 11:3b241ecb75ed | 535 | isTimeoutRunning = true; |
danstrider | 11:3b241ecb75ed | 536 | |
danstrider | 11:3b241ecb75ed | 537 | // what is active? |
mkelly10 | 12:a0519d11d2b6 | 538 | bce().pause(); |
mkelly10 | 12:a0519d11d2b6 | 539 | batt().pause(); |
danstrider | 11:3b241ecb75ed | 540 | } |
danstrider | 11:3b241ecb75ed | 541 | // how exit? |
danstrider | 11:3b241ecb75ed | 542 | if (pc().readable()) { |
danstrider | 11:3b241ecb75ed | 543 | state = KEYBOARD; |
danstrider | 11:3b241ecb75ed | 544 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 545 | } |
danstrider | 11:3b241ecb75ed | 546 | break; |
danstrider | 11:3b241ecb75ed | 547 | |
danstrider | 11:3b241ecb75ed | 548 | case KEYBOARD : |
danstrider | 11:3b241ecb75ed | 549 | pc().printf("\r\nstate: KEYBOARD\r\n"); |
danstrider | 11:3b241ecb75ed | 550 | if (pc().readable()) { |
danstrider | 11:3b241ecb75ed | 551 | state = keyboard(); // get new state command |
danstrider | 11:3b241ecb75ed | 552 | if (state == -1) { // error, that wasn't a new state command |
danstrider | 11:3b241ecb75ed | 553 | state = SIT_IDLE; |
danstrider | 11:3b241ecb75ed | 554 | } |
danstrider | 11:3b241ecb75ed | 555 | //pc().printf("new state is: %d \r\n",state); |
danstrider | 11:3b241ecb75ed | 556 | } |
danstrider | 11:3b241ecb75ed | 557 | break; |
danstrider | 10:085ab7328054 | 558 | |
danstrider | 11:3b241ecb75ed | 559 | case EMERGENCY_CLIMB : |
danstrider | 11:3b241ecb75ed | 560 | // start local state timer and init any other one-shot actions |
danstrider | 11:3b241ecb75ed | 561 | if (!isTimeoutRunning) { |
danstrider | 11:3b241ecb75ed | 562 | pc().printf("\r\nstate: EMERGENCY_CLIMB\r\n"); |
danstrider | 11:3b241ecb75ed | 563 | timer.reset(); // timer goes back to zero |
danstrider | 11:3b241ecb75ed | 564 | timer.start(); // background timer starts running |
danstrider | 11:3b241ecb75ed | 565 | isTimeoutRunning = true; |
danstrider | 11:3b241ecb75ed | 566 | |
danstrider | 11:3b241ecb75ed | 567 | // what needs to be started? |
mkelly10 | 12:a0519d11d2b6 | 568 | bce().unpause(); |
mkelly10 | 12:a0519d11d2b6 | 569 | batt().unpause(); |
danstrider | 11:3b241ecb75ed | 570 | |
danstrider | 11:3b241ecb75ed | 571 | // what is active? |
danstrider | 11:3b241ecb75ed | 572 | bce().setPosition_mm(bce().getTravelLimit()); |
danstrider | 11:3b241ecb75ed | 573 | batt().setPosition_mm(0.0); |
danstrider | 11:3b241ecb75ed | 574 | } |
danstrider | 11:3b241ecb75ed | 575 | // how exit? |
danstrider | 11:3b241ecb75ed | 576 | if (timer > timeout) { |
danstrider | 11:3b241ecb75ed | 577 | pc().printf("timed out\r\n"); |
danstrider | 11:3b241ecb75ed | 578 | state = FLOAT_LEVEL; |
danstrider | 11:3b241ecb75ed | 579 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 580 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 581 | } |
tnhnrl | 13:84fcbe1dcd62 | 582 | else if (depthLoop().getPosition() < 0.2) { |
tnhnrl | 13:84fcbe1dcd62 | 583 | pc().printf("depth: %3.1f, cmd: 0.5\r\n",depthLoop().getPosition()); |
danstrider | 11:3b241ecb75ed | 584 | state = FLOAT_LEVEL; |
danstrider | 11:3b241ecb75ed | 585 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 586 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 587 | } |
danstrider | 11:3b241ecb75ed | 588 | break; |
danstrider | 11:3b241ecb75ed | 589 | |
danstrider | 11:3b241ecb75ed | 590 | case FIND_NEUTRAL : |
danstrider | 11:3b241ecb75ed | 591 | // start local state timer and init any other one-shot actions |
danstrider | 11:3b241ecb75ed | 592 | if (!isTimeoutRunning) { |
danstrider | 11:3b241ecb75ed | 593 | pc().printf("\r\nstate: FIND_NEUTRAL\r\n"); |
danstrider | 11:3b241ecb75ed | 594 | timer.reset(); // timer goes back to zero |
danstrider | 11:3b241ecb75ed | 595 | timer.start(); // background timer starts running |
danstrider | 11:3b241ecb75ed | 596 | isTimeoutRunning = true; |
danstrider | 11:3b241ecb75ed | 597 | |
danstrider | 11:3b241ecb75ed | 598 | // what needs to be started? |
mkelly10 | 12:a0519d11d2b6 | 599 | bce().unpause(); |
mkelly10 | 12:a0519d11d2b6 | 600 | batt().unpause(); |
tnhnrl | 13:84fcbe1dcd62 | 601 | |
danstrider | 11:3b241ecb75ed | 602 | // what is active? |
danstrider | 11:3b241ecb75ed | 603 | depthLoop().setCommand(depthCommand); |
danstrider | 11:3b241ecb75ed | 604 | pitchLoop().setCommand(pitchCommand); |
danstrider | 11:3b241ecb75ed | 605 | } |
danstrider | 11:3b241ecb75ed | 606 | // how exit? |
danstrider | 11:3b241ecb75ed | 607 | if (timer > timeout) { |
danstrider | 11:3b241ecb75ed | 608 | pc().printf("timed out\r\n"); |
danstrider | 11:3b241ecb75ed | 609 | state = FLOAT_LEVEL; |
danstrider | 11:3b241ecb75ed | 610 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 611 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 612 | } |
tnhnrl | 13:84fcbe1dcd62 | 613 | else if ((abs(depthLoop().getPosition() - depthLoop().getCommand()) < depthTolerance) and |
danstrider | 11:3b241ecb75ed | 614 | (abs(imu().getPitch() - pitchLoop().getCommand()) < pitchTolerance)) { |
danstrider | 11:3b241ecb75ed | 615 | state = RISE; |
danstrider | 11:3b241ecb75ed | 616 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 617 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 618 | } |
danstrider | 11:3b241ecb75ed | 619 | // what is active? |
tnhnrl | 13:84fcbe1dcd62 | 620 | pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition()); |
danstrider | 11:3b241ecb75ed | 621 | bce().setPosition_mm(depthLoop().getOutput()); |
danstrider | 11:3b241ecb75ed | 622 | batt().setPosition_mm(pitchLoop().getOutput()); |
danstrider | 11:3b241ecb75ed | 623 | break; |
mkelly10 | 5:15bd96205bb2 | 624 | |
danstrider | 11:3b241ecb75ed | 625 | case DIVE : |
danstrider | 11:3b241ecb75ed | 626 | // start local state timer and init any other one-shot actions |
danstrider | 11:3b241ecb75ed | 627 | if (!isTimeoutRunning) { |
danstrider | 11:3b241ecb75ed | 628 | pc().printf("\r\nstate: DIVE\r\n"); |
danstrider | 11:3b241ecb75ed | 629 | timer.reset(); // timer goes back to zero |
danstrider | 11:3b241ecb75ed | 630 | timer.start(); // background timer starts running |
danstrider | 11:3b241ecb75ed | 631 | isTimeoutRunning = true; |
danstrider | 11:3b241ecb75ed | 632 | |
danstrider | 11:3b241ecb75ed | 633 | // what needs to be started? |
mkelly10 | 12:a0519d11d2b6 | 634 | bce().unpause(); |
mkelly10 | 12:a0519d11d2b6 | 635 | batt().unpause(); |
danstrider | 11:3b241ecb75ed | 636 | |
danstrider | 11:3b241ecb75ed | 637 | // what are the commands? |
danstrider | 11:3b241ecb75ed | 638 | depthLoop().setCommand(depthCommand); |
danstrider | 11:3b241ecb75ed | 639 | pitchLoop().setCommand(pitchCommand); |
danstrider | 11:3b241ecb75ed | 640 | pc().printf("depth cmd: %3.1f\r\n",depthLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 641 | pc().printf("pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 642 | } |
danstrider | 11:3b241ecb75ed | 643 | // how exit? |
danstrider | 11:3b241ecb75ed | 644 | if (timer > timeout) { |
danstrider | 11:3b241ecb75ed | 645 | pc().printf("timed out\r\n"); |
danstrider | 11:3b241ecb75ed | 646 | state = FLOAT_LEVEL; |
danstrider | 11:3b241ecb75ed | 647 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 648 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 649 | } |
tnhnrl | 13:84fcbe1dcd62 | 650 | else if (depthLoop().getPosition() > depthLoop().getCommand()) { |
tnhnrl | 13:84fcbe1dcd62 | 651 | pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 652 | state = RISE; |
danstrider | 11:3b241ecb75ed | 653 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 654 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 655 | } |
danstrider | 11:3b241ecb75ed | 656 | // what is active? |
tnhnrl | 13:84fcbe1dcd62 | 657 | pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition()); |
danstrider | 11:3b241ecb75ed | 658 | bce().setPosition_mm(depthLoop().getOutput()); |
danstrider | 11:3b241ecb75ed | 659 | batt().setPosition_mm(pitchLoop().getOutput()); |
danstrider | 11:3b241ecb75ed | 660 | break; |
danstrider | 11:3b241ecb75ed | 661 | |
danstrider | 11:3b241ecb75ed | 662 | case RISE : |
danstrider | 11:3b241ecb75ed | 663 | // start local state timer and init any other one-shot actions |
danstrider | 11:3b241ecb75ed | 664 | if (!isTimeoutRunning) { |
danstrider | 11:3b241ecb75ed | 665 | pc().printf("\r\nstate: RISE\r\n"); |
danstrider | 11:3b241ecb75ed | 666 | timer.reset(); // timer goes back to zero |
danstrider | 11:3b241ecb75ed | 667 | timer.start(); // background timer starts running |
danstrider | 11:3b241ecb75ed | 668 | isTimeoutRunning = true; |
danstrider | 11:3b241ecb75ed | 669 | |
danstrider | 11:3b241ecb75ed | 670 | // what needs to be started? |
mkelly10 | 12:a0519d11d2b6 | 671 | bce().unpause(); |
mkelly10 | 12:a0519d11d2b6 | 672 | batt().unpause(); |
tnhnrl | 13:84fcbe1dcd62 | 673 | |
danstrider | 11:3b241ecb75ed | 674 | // what are the commands? |
danstrider | 11:3b241ecb75ed | 675 | depthLoop().setCommand(0.0); |
danstrider | 11:3b241ecb75ed | 676 | pitchLoop().setCommand(-pitchCommand); |
danstrider | 11:3b241ecb75ed | 677 | pc().printf("depth cmd: 0.0\r\n"); |
danstrider | 11:3b241ecb75ed | 678 | pc().printf("pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 679 | } |
danstrider | 11:3b241ecb75ed | 680 | // how exit? |
danstrider | 11:3b241ecb75ed | 681 | if (timer > timeout) { |
danstrider | 11:3b241ecb75ed | 682 | pc().printf("timed out\r\n"); |
danstrider | 11:3b241ecb75ed | 683 | state = FLOAT_LEVEL; |
danstrider | 11:3b241ecb75ed | 684 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 685 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 686 | } |
tnhnrl | 13:84fcbe1dcd62 | 687 | else if (depthLoop().getPosition() < depthLoop().getCommand()) { |
tnhnrl | 13:84fcbe1dcd62 | 688 | pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); |
danstrider | 11:3b241ecb75ed | 689 | state = FLOAT_LEVEL; |
danstrider | 11:3b241ecb75ed | 690 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 691 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 692 | } |
tnhnrl | 13:84fcbe1dcd62 | 693 | pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition()); |
danstrider | 11:3b241ecb75ed | 694 | // what is active? |
danstrider | 11:3b241ecb75ed | 695 | bce().setPosition_mm(depthLoop().getOutput()); |
danstrider | 11:3b241ecb75ed | 696 | batt().setPosition_mm(pitchLoop().getOutput()); |
danstrider | 11:3b241ecb75ed | 697 | break; |
danstrider | 11:3b241ecb75ed | 698 | |
danstrider | 11:3b241ecb75ed | 699 | case FLOAT_LEVEL : |
danstrider | 11:3b241ecb75ed | 700 | // start local state timer and init any other one-shot actions |
danstrider | 11:3b241ecb75ed | 701 | if (!isTimeoutRunning) { |
danstrider | 11:3b241ecb75ed | 702 | pc().printf("\r\nstate: FLOAT_LEVEL\r\n"); |
danstrider | 11:3b241ecb75ed | 703 | timer.reset(); // timer goes back to zero |
danstrider | 11:3b241ecb75ed | 704 | timer.start(); // background timer starts running |
danstrider | 11:3b241ecb75ed | 705 | isTimeoutRunning = true; |
danstrider | 11:3b241ecb75ed | 706 | |
danstrider | 11:3b241ecb75ed | 707 | // what needs to be started? |
mkelly10 | 12:a0519d11d2b6 | 708 | bce().unpause(); |
mkelly10 | 12:a0519d11d2b6 | 709 | batt().unpause(); |
tnhnrl | 13:84fcbe1dcd62 | 710 | |
danstrider | 11:3b241ecb75ed | 711 | // what are the commands |
danstrider | 11:3b241ecb75ed | 712 | bce().setPosition_mm(bceFloatPosition); |
danstrider | 11:3b241ecb75ed | 713 | pitchLoop().setCommand(0.0); |
danstrider | 11:3b241ecb75ed | 714 | } |
danstrider | 11:3b241ecb75ed | 715 | // how exit? |
danstrider | 11:3b241ecb75ed | 716 | if (timer > timeout) { |
danstrider | 11:3b241ecb75ed | 717 | pc().printf("timed out\r\n"); |
danstrider | 11:3b241ecb75ed | 718 | state = FLOAT_BROADCAST; |
danstrider | 11:3b241ecb75ed | 719 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 720 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 721 | } |
danstrider | 11:3b241ecb75ed | 722 | else if (abs(imu().getPitch() - pitchLoop().getCommand()) < abs(pitchTolerance)) { |
danstrider | 11:3b241ecb75ed | 723 | pc().printf("pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), pitchTolerance); |
danstrider | 11:3b241ecb75ed | 724 | state = FLOAT_BROADCAST; |
danstrider | 11:3b241ecb75ed | 725 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 726 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 727 | } |
danstrider | 11:3b241ecb75ed | 728 | // what is active? |
danstrider | 11:3b241ecb75ed | 729 | pc().printf("pitchLoop output: %3.1f, batt pos: %3.1f, piston pos: %3.1f\r", pitchLoop().getOutput(), batt().getPosition_mm(), bce().getPosition_mm()); |
danstrider | 11:3b241ecb75ed | 730 | batt().setPosition_mm(pitchLoop().getOutput()); |
danstrider | 11:3b241ecb75ed | 731 | break; |
danstrider | 11:3b241ecb75ed | 732 | |
danstrider | 11:3b241ecb75ed | 733 | case FLOAT_BROADCAST : |
danstrider | 11:3b241ecb75ed | 734 | // start local state timer and init any other one-shot actions |
danstrider | 11:3b241ecb75ed | 735 | if (!isTimeoutRunning) { |
danstrider | 11:3b241ecb75ed | 736 | pc().printf("\r\nstate: FLOAT_BROADCAST\r\n"); |
danstrider | 11:3b241ecb75ed | 737 | timer.reset(); // timer goes back to zero |
danstrider | 11:3b241ecb75ed | 738 | timer.start(); // background timer starts running |
danstrider | 11:3b241ecb75ed | 739 | isTimeoutRunning = true; |
danstrider | 11:3b241ecb75ed | 740 | |
danstrider | 11:3b241ecb75ed | 741 | // what needs to be started? |
mkelly10 | 12:a0519d11d2b6 | 742 | bce().unpause(); |
mkelly10 | 12:a0519d11d2b6 | 743 | batt().unpause(); |
danstrider | 11:3b241ecb75ed | 744 | |
danstrider | 11:3b241ecb75ed | 745 | // what are the commands? |
danstrider | 11:3b241ecb75ed | 746 | bce().setPosition_mm(bceFloatPosition); |
danstrider | 11:3b241ecb75ed | 747 | batt().setPosition_mm(battFloatPosition); |
danstrider | 11:3b241ecb75ed | 748 | } |
danstrider | 11:3b241ecb75ed | 749 | // how exit? |
danstrider | 11:3b241ecb75ed | 750 | if (timer > timeout) { |
danstrider | 11:3b241ecb75ed | 751 | pc().printf("timed out\r\n"); |
danstrider | 11:3b241ecb75ed | 752 | state = SIT_IDLE; |
danstrider | 11:3b241ecb75ed | 753 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 754 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 755 | } |
danstrider | 11:3b241ecb75ed | 756 | if (abs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) { |
danstrider | 11:3b241ecb75ed | 757 | pc().printf("position: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(), bce().getDeadband()); |
danstrider | 11:3b241ecb75ed | 758 | state = SIT_IDLE; |
danstrider | 11:3b241ecb75ed | 759 | timer.reset(); |
danstrider | 11:3b241ecb75ed | 760 | isTimeoutRunning = false; |
danstrider | 11:3b241ecb75ed | 761 | } |
tnhnrl | 13:84fcbe1dcd62 | 762 | pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition()); |
danstrider | 11:3b241ecb75ed | 763 | break; |
danstrider | 11:3b241ecb75ed | 764 | |
danstrider | 11:3b241ecb75ed | 765 | default : |
danstrider | 11:3b241ecb75ed | 766 | state = SIT_IDLE; |
danstrider | 11:3b241ecb75ed | 767 | } |
danstrider | 10:085ab7328054 | 768 | } |
tzyoung | 0:ea293bbf9717 | 769 | |
danstrider | 11:3b241ecb75ed | 770 | |
danstrider | 10:085ab7328054 | 771 | int main() { |
danstrider | 10:085ab7328054 | 772 | setup(); |
danstrider | 10:085ab7328054 | 773 | while(1) { |
danstrider | 11:3b241ecb75ed | 774 | led1() = !led1(); // blink |
danstrider | 11:3b241ecb75ed | 775 | //pc().printf("roll: %3.1f, pitch %3.1f, yaw: %3.1f\r", imu().getRoll(), imu().getPitch(), imu().getHeading()); |
danstrider | 11:3b241ecb75ed | 776 | stateMachine(); |
danstrider | 10:085ab7328054 | 777 | } |
danstrider | 10:085ab7328054 | 778 | } |