Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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

StateMachine/StateMachine.cpp@16:3363b9f14913, 2017-11-06 (annotated)

Committer:
tnhnrl
Date:
Mon Nov 06 22:57:56 2017 +0000
Revision:
16:3363b9f14913
Child:
17:7c16b5671d0e
Update to FSG test program used in the LASR pool

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tnhnrl 16:3363b9f14913 1 #include "StateMachine.hpp"
tnhnrl 16:3363b9f14913 2 #include "StaticDefs.hpp"
tnhnrl 16:3363b9f14913 3
tnhnrl 16:3363b9f14913 4 StateMachine::StateMachine() {
tnhnrl 16:3363b9f14913 5 timeout = 60; // generic timeout for every state, seconds
tnhnrl 16:3363b9f14913 6 depthTolerance = 0.25; // depth tolerance for neutral finding exit critera
tnhnrl 16:3363b9f14913 7 pitchTolerance = 1.0; // pitch angle tolerance for neutral finding exit criteria
tnhnrl 16:3363b9f14913 8 bceFloatPosition = 300; // bce position for "float" states
tnhnrl 16:3363b9f14913 9 battFloatPosition = 50; // batt position for "broadcast" state
tnhnrl 16:3363b9f14913 10
tnhnrl 16:3363b9f14913 11 depthCommand = 3.5; // user keyboard depth
tnhnrl 16:3363b9f14913 12 pitchCommand = -20.0; // user keyboard depth
tnhnrl 16:3363b9f14913 13 }
tnhnrl 16:3363b9f14913 14
tnhnrl 16:3363b9f14913 15 void StateMachine::runStateMachine() {
tnhnrl 16:3363b9f14913 16 static int state = SIT_IDLE; // select starting state here
tnhnrl 16:3363b9f14913 17 static bool isTimeoutRunning = false; // default timer to not running
tnhnrl 16:3363b9f14913 18
tnhnrl 16:3363b9f14913 19 // finite state machine ... each state has at least one exit criteria
tnhnrl 16:3363b9f14913 20 switch (state) {
tnhnrl 16:3363b9f14913 21 case SIT_IDLE :
tnhnrl 16:3363b9f14913 22 // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
tnhnrl 16:3363b9f14913 23 if (!isTimeoutRunning) {
tnhnrl 16:3363b9f14913 24 showMenu();
tnhnrl 16:3363b9f14913 25 pc().printf("\r\n\nstate: SIT_IDLE\r\n");
tnhnrl 16:3363b9f14913 26 isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 27
tnhnrl 16:3363b9f14913 28 // what is active?
tnhnrl 16:3363b9f14913 29 bce().pause();
tnhnrl 16:3363b9f14913 30 batt().pause();
tnhnrl 16:3363b9f14913 31 }
tnhnrl 16:3363b9f14913 32 // how exit?
tnhnrl 16:3363b9f14913 33 if (pc().readable()) {
tnhnrl 16:3363b9f14913 34 state = KEYBOARD;
tnhnrl 16:3363b9f14913 35 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 36 }
tnhnrl 16:3363b9f14913 37 break;
tnhnrl 16:3363b9f14913 38
tnhnrl 16:3363b9f14913 39 case KEYBOARD :
tnhnrl 16:3363b9f14913 40 pc().printf("\r\n\nstate: KEYBOARD\r\n");
tnhnrl 16:3363b9f14913 41 if (pc().readable()) {
tnhnrl 16:3363b9f14913 42 state = keyboard(); // get new state command
tnhnrl 16:3363b9f14913 43 if (state == -1) { // error, that wasn't a new state command
tnhnrl 16:3363b9f14913 44 state = SIT_IDLE;
tnhnrl 16:3363b9f14913 45 }
tnhnrl 16:3363b9f14913 46 //pc().printf("new state is: %d \r\n",state);
tnhnrl 16:3363b9f14913 47 }
tnhnrl 16:3363b9f14913 48 break;
tnhnrl 16:3363b9f14913 49
tnhnrl 16:3363b9f14913 50 case EMERGENCY_CLIMB :
tnhnrl 16:3363b9f14913 51 // start local state timer and init any other one-shot actions
tnhnrl 16:3363b9f14913 52 if (!isTimeoutRunning) {
tnhnrl 16:3363b9f14913 53 pc().printf("\r\n\nstate: EMERGENCY_CLIMB\r\n");
tnhnrl 16:3363b9f14913 54 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 55 timer.start(); // background timer starts running
tnhnrl 16:3363b9f14913 56 isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 57
tnhnrl 16:3363b9f14913 58 // what needs to be started?
tnhnrl 16:3363b9f14913 59 bce().unpause();
tnhnrl 16:3363b9f14913 60 batt().unpause();
tnhnrl 16:3363b9f14913 61
tnhnrl 16:3363b9f14913 62 // what is active?
tnhnrl 16:3363b9f14913 63 bce().setPosition_mm(bce().getTravelLimit());
tnhnrl 16:3363b9f14913 64 batt().setPosition_mm(0.0);
tnhnrl 16:3363b9f14913 65 }
tnhnrl 16:3363b9f14913 66 // how exit?
tnhnrl 16:3363b9f14913 67 if (timer > timeout) {
tnhnrl 16:3363b9f14913 68 pc().printf("EC: timed out\r\n");
tnhnrl 16:3363b9f14913 69 state = FLOAT_LEVEL;
tnhnrl 16:3363b9f14913 70 timer.reset();
tnhnrl 16:3363b9f14913 71 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 72 }
tnhnrl 16:3363b9f14913 73 else if (depthLoop().getPosition() < 0.2) {
tnhnrl 16:3363b9f14913 74 pc().printf("EC: depth: %3.1f, cmd: 0.5 [%0.1f sec]\r",depthLoop().getPosition(), timer.read());
tnhnrl 16:3363b9f14913 75 state = FLOAT_LEVEL;
tnhnrl 16:3363b9f14913 76 timer.reset();
tnhnrl 16:3363b9f14913 77 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 78 }
tnhnrl 16:3363b9f14913 79 break;
tnhnrl 16:3363b9f14913 80
tnhnrl 16:3363b9f14913 81 case FIND_NEUTRAL :
tnhnrl 16:3363b9f14913 82 // start local state timer and init any other one-shot actions
tnhnrl 16:3363b9f14913 83 if (!isTimeoutRunning) {
tnhnrl 16:3363b9f14913 84 pc().printf("\r\n\nstate: FIND_NEUTRAL\r\n");
tnhnrl 16:3363b9f14913 85 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 86 timer.start(); // background timer starts running
tnhnrl 16:3363b9f14913 87 isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 88
tnhnrl 16:3363b9f14913 89 // what needs to be started?
tnhnrl 16:3363b9f14913 90 bce().unpause();
tnhnrl 16:3363b9f14913 91 batt().unpause();
tnhnrl 16:3363b9f14913 92
tnhnrl 16:3363b9f14913 93 // what is active?
tnhnrl 16:3363b9f14913 94 depthLoop().setCommand(depthCommand);
tnhnrl 16:3363b9f14913 95 pitchLoop().setCommand(0.0);
tnhnrl 16:3363b9f14913 96 }
tnhnrl 16:3363b9f14913 97 // how exit?
tnhnrl 16:3363b9f14913 98 if (timer > timeout) {
tnhnrl 16:3363b9f14913 99 pc().printf("FN: timed out\r\n");
tnhnrl 16:3363b9f14913 100 state = FLOAT_LEVEL;
tnhnrl 16:3363b9f14913 101 timer.reset();
tnhnrl 16:3363b9f14913 102 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 103 }
tnhnrl 16:3363b9f14913 104
tnhnrl 16:3363b9f14913 105 //depth tolerance of 0.25 feet, pitch tolerance of 1.0 degree
tnhnrl 16:3363b9f14913 106 else if ((abs(depthLoop().getPosition() - depthLoop().getCommand()) < depthTolerance) and
tnhnrl 16:3363b9f14913 107 (abs(imu().getPitch() - pitchLoop().getCommand()) < pitchTolerance)) {
tnhnrl 16:3363b9f14913 108 state = RISE;
tnhnrl 16:3363b9f14913 109 timer.reset();
tnhnrl 16:3363b9f14913 110 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 111 }
tnhnrl 16:3363b9f14913 112 // what is active?
tnhnrl 16:3363b9f14913 113 pc().printf("FN: bce pos: %3.1f mm, batt pos: %3.1f mm (pitchLoop: 0.0 deg)(depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
tnhnrl 16:3363b9f14913 114 bce().setPosition_mm(depthLoop().getOutput());
tnhnrl 16:3363b9f14913 115 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 16:3363b9f14913 116 break;
tnhnrl 16:3363b9f14913 117
tnhnrl 16:3363b9f14913 118 case DIVE :
tnhnrl 16:3363b9f14913 119 // start local state timer and init any other one-shot actions
tnhnrl 16:3363b9f14913 120 if (!isTimeoutRunning) {
tnhnrl 16:3363b9f14913 121 pc().printf("\r\n\nstate: DIVE\r\n");
tnhnrl 16:3363b9f14913 122 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 123 timer.start(); // background timer starts running
tnhnrl 16:3363b9f14913 124 isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 125
tnhnrl 16:3363b9f14913 126 // what needs to be started?
tnhnrl 16:3363b9f14913 127 bce().unpause();
tnhnrl 16:3363b9f14913 128 batt().unpause();
tnhnrl 16:3363b9f14913 129
tnhnrl 16:3363b9f14913 130 // what are the commands?
tnhnrl 16:3363b9f14913 131 depthLoop().setCommand(depthCommand);
tnhnrl 16:3363b9f14913 132 pitchLoop().setCommand(pitchCommand);
tnhnrl 16:3363b9f14913 133 pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
tnhnrl 16:3363b9f14913 134 pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 16:3363b9f14913 135 }
tnhnrl 16:3363b9f14913 136 // how exit?
tnhnrl 16:3363b9f14913 137 if (timer > timeout) {
tnhnrl 16:3363b9f14913 138 pc().printf("DIVE: timed out\r\n");
tnhnrl 16:3363b9f14913 139 state = EMERGENCY_CLIMB;
tnhnrl 16:3363b9f14913 140 timer.reset();
tnhnrl 16:3363b9f14913 141 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 142 }
tnhnrl 16:3363b9f14913 143 else if (depthLoop().getPosition() > depthLoop().getCommand()) {
tnhnrl 16:3363b9f14913 144 pc().printf("DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 16:3363b9f14913 145 state = RISE;
tnhnrl 16:3363b9f14913 146 timer.reset();
tnhnrl 16:3363b9f14913 147 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 148 }
tnhnrl 16:3363b9f14913 149 // what is active?
tnhnrl 16:3363b9f14913 150 pc().printf("DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
tnhnrl 16:3363b9f14913 151 bce().setPosition_mm(depthLoop().getOutput());
tnhnrl 16:3363b9f14913 152 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 16:3363b9f14913 153 break;
tnhnrl 16:3363b9f14913 154
tnhnrl 16:3363b9f14913 155 case RISE :
tnhnrl 16:3363b9f14913 156 // start local state timer and init any other one-shot actions
tnhnrl 16:3363b9f14913 157 if (!isTimeoutRunning) {
tnhnrl 16:3363b9f14913 158 pc().printf("\r\n\nstate: RISE\r\n");
tnhnrl 16:3363b9f14913 159 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 160 timer.start(); // background timer starts running
tnhnrl 16:3363b9f14913 161 isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 162
tnhnrl 16:3363b9f14913 163 // what needs to be started?
tnhnrl 16:3363b9f14913 164 bce().unpause();
tnhnrl 16:3363b9f14913 165 batt().unpause();
tnhnrl 16:3363b9f14913 166
tnhnrl 16:3363b9f14913 167 // what are the commands?
tnhnrl 16:3363b9f14913 168 depthLoop().setCommand(0.0);
tnhnrl 16:3363b9f14913 169 pitchLoop().setCommand(-pitchCommand);
tnhnrl 16:3363b9f14913 170 pc().printf("RISE: depth cmd: 0.0\r\n");
tnhnrl 16:3363b9f14913 171 pc().printf("RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 16:3363b9f14913 172 }
tnhnrl 16:3363b9f14913 173 // how exit?
tnhnrl 16:3363b9f14913 174 if (timer > timeout) {
tnhnrl 16:3363b9f14913 175 pc().printf("RISE: timed out\r\n");
tnhnrl 16:3363b9f14913 176 state = FLOAT_LEVEL;
tnhnrl 16:3363b9f14913 177 timer.reset();
tnhnrl 16:3363b9f14913 178 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 179 }
tnhnrl 16:3363b9f14913 180 else if (depthLoop().getPosition() < depthLoop().getCommand()) {
tnhnrl 16:3363b9f14913 181 pc().printf("RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
tnhnrl 16:3363b9f14913 182 state = FLOAT_LEVEL;
tnhnrl 16:3363b9f14913 183 timer.reset();
tnhnrl 16:3363b9f14913 184 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 185 }
tnhnrl 16:3363b9f14913 186 pc().printf("RISE: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
tnhnrl 16:3363b9f14913 187 // what is active?
tnhnrl 16:3363b9f14913 188 bce().setPosition_mm(depthLoop().getOutput());
tnhnrl 16:3363b9f14913 189 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 16:3363b9f14913 190 break;
tnhnrl 16:3363b9f14913 191
tnhnrl 16:3363b9f14913 192 case FLOAT_LEVEL :
tnhnrl 16:3363b9f14913 193 // start local state timer and init any other one-shot actions
tnhnrl 16:3363b9f14913 194 if (!isTimeoutRunning) {
tnhnrl 16:3363b9f14913 195 pc().printf("\r\n\nstate: FLOAT_LEVEL\r\n");
tnhnrl 16:3363b9f14913 196 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 197 timer.start(); // background timer starts running
tnhnrl 16:3363b9f14913 198 isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 199
tnhnrl 16:3363b9f14913 200 // what needs to be started?
tnhnrl 16:3363b9f14913 201 bce().unpause();
tnhnrl 16:3363b9f14913 202 batt().unpause();
tnhnrl 16:3363b9f14913 203
tnhnrl 16:3363b9f14913 204 // what are the commands
tnhnrl 16:3363b9f14913 205 bce().setPosition_mm(bceFloatPosition);
tnhnrl 16:3363b9f14913 206 pitchLoop().setCommand(0.0);
tnhnrl 16:3363b9f14913 207 }
tnhnrl 16:3363b9f14913 208 // how exit?
tnhnrl 16:3363b9f14913 209 if (timer > timeout) {
tnhnrl 16:3363b9f14913 210 pc().printf("FL: timed out\r\n");
tnhnrl 16:3363b9f14913 211 state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 212 timer.reset();
tnhnrl 16:3363b9f14913 213 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 214 }
tnhnrl 16:3363b9f14913 215 else if (abs(imu().getPitch() - pitchLoop().getCommand()) < abs(pitchTolerance)) {
tnhnrl 16:3363b9f14913 216 pc().printf("FL: pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), pitchTolerance);
tnhnrl 16:3363b9f14913 217 state = FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 218 timer.reset();
tnhnrl 16:3363b9f14913 219 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 220 }
tnhnrl 16:3363b9f14913 221 // what is active?
tnhnrl 16:3363b9f14913 222 pc().printf("FL: pitchLoop output: %3.1f, batt pos: %3.1f, piston pos: %3.1f [%0.1f sec]\r", pitchLoop().getOutput(), batt().getPosition_mm(), bce().getPosition_mm(), timer.read());
tnhnrl 16:3363b9f14913 223 batt().setPosition_mm(pitchLoop().getOutput());
tnhnrl 16:3363b9f14913 224 break;
tnhnrl 16:3363b9f14913 225
tnhnrl 16:3363b9f14913 226 case FLOAT_BROADCAST :
tnhnrl 16:3363b9f14913 227 // start local state timer and init any other one-shot actions
tnhnrl 16:3363b9f14913 228 if (!isTimeoutRunning) {
tnhnrl 16:3363b9f14913 229 pc().printf("\r\n\nstate: FLOAT_BROADCAST\r\n");
tnhnrl 16:3363b9f14913 230 timer.reset(); // timer goes back to zero
tnhnrl 16:3363b9f14913 231 timer.start(); // background timer starts running
tnhnrl 16:3363b9f14913 232 isTimeoutRunning = true;
tnhnrl 16:3363b9f14913 233
tnhnrl 16:3363b9f14913 234 // what needs to be started?
tnhnrl 16:3363b9f14913 235 bce().unpause();
tnhnrl 16:3363b9f14913 236 batt().unpause();
tnhnrl 16:3363b9f14913 237
tnhnrl 16:3363b9f14913 238 // what are the commands?
tnhnrl 16:3363b9f14913 239 bce().setPosition_mm(bceFloatPosition);
tnhnrl 16:3363b9f14913 240 batt().setPosition_mm(battFloatPosition);
tnhnrl 16:3363b9f14913 241 }
tnhnrl 16:3363b9f14913 242 // how exit?
tnhnrl 16:3363b9f14913 243 if (timer > timeout) {
tnhnrl 16:3363b9f14913 244 pc().printf("FB: timed out\r\n");
tnhnrl 16:3363b9f14913 245 state = SIT_IDLE;
tnhnrl 16:3363b9f14913 246 timer.reset();
tnhnrl 16:3363b9f14913 247 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 248 }
tnhnrl 16:3363b9f14913 249 if ( (abs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and
tnhnrl 16:3363b9f14913 250 (abs(batt().getPosition_mm() - batt().getSetPosition_mm()) < batt().getDeadband()) ) {
tnhnrl 16:3363b9f14913 251 pc().printf("FB: position: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(), bce().getDeadband());
tnhnrl 16:3363b9f14913 252 state = SIT_IDLE;
tnhnrl 16:3363b9f14913 253 timer.reset();
tnhnrl 16:3363b9f14913 254 isTimeoutRunning = false;
tnhnrl 16:3363b9f14913 255 }
tnhnrl 16:3363b9f14913 256 pc().printf("FB: bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) [%0.1f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
tnhnrl 16:3363b9f14913 257 break;
tnhnrl 16:3363b9f14913 258
tnhnrl 16:3363b9f14913 259 default :
tnhnrl 16:3363b9f14913 260 state = SIT_IDLE;
tnhnrl 16:3363b9f14913 261 }
tnhnrl 16:3363b9f14913 262 }
tnhnrl 16:3363b9f14913 263
tnhnrl 16:3363b9f14913 264 // output the keyboard menu for user's reference
tnhnrl 16:3363b9f14913 265 void StateMachine::showMenu() {
tnhnrl 16:3363b9f14913 266 pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n");
tnhnrl 16:3363b9f14913 267 pc().printf(" N to find neutral\r\n");
tnhnrl 16:3363b9f14913 268 pc().printf(" D to initiate dive cycle\r\n");
tnhnrl 16:3363b9f14913 269 pc().printf(" R to initiate rise\r\n");
tnhnrl 16:3363b9f14913 270 pc().printf(" L to float level\r\n");
tnhnrl 16:3363b9f14913 271 pc().printf(" B to float at broadcast pitch\r\n");
tnhnrl 16:3363b9f14913 272 pc().printf(" E to initiate emergency climb\r\n");
tnhnrl 16:3363b9f14913 273 pc().printf(" H to run homing sequence on both BCE and Batt\r\n");
tnhnrl 16:3363b9f14913 274 pc().printf(" T to tare the depth sensor\r\n");
tnhnrl 16:3363b9f14913 275 pc().printf("[/] to change bce neutral position\r\n");
tnhnrl 16:3363b9f14913 276 pc().printf("</> to change batt neutral position\r\n");
tnhnrl 16:3363b9f14913 277 pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",pitchCommand);
tnhnrl 16:3363b9f14913 278 pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",depthCommand);
tnhnrl 16:3363b9f14913 279 //pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 16:3363b9f14913 280 //pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",depthLoop().getCommand());
tnhnrl 16:3363b9f14913 281 pc().printf("+/- to decrease/increase timeout: %d s\r\n",timeout);
tnhnrl 16:3363b9f14913 282 pc().printf(" 1 BCE PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 283 pc().printf(" 2 BATT PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 284 pc().printf(" 3 Depth PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 285 pc().printf(" 4 Pitch PID sub-menu\r\n");
tnhnrl 16:3363b9f14913 286 pc().printf(" C See sensor readings\r\n");
tnhnrl 16:3363b9f14913 287 pc().printf(" ? to reset mbed\r\n");
tnhnrl 16:3363b9f14913 288 }
tnhnrl 16:3363b9f14913 289
tnhnrl 16:3363b9f14913 290 // keyboard currently handles a key at a time
tnhnrl 16:3363b9f14913 291 // returns -1 if not a state command
tnhnrl 16:3363b9f14913 292 // returns a positive number to command a new state
tnhnrl 16:3363b9f14913 293 int StateMachine::keyboard() {
tnhnrl 16:3363b9f14913 294 char userInput;
tnhnrl 16:3363b9f14913 295
tnhnrl 16:3363b9f14913 296 // check keyboard and make settings changes as requested
tnhnrl 16:3363b9f14913 297 if (pc().readable()) {
tnhnrl 16:3363b9f14913 298 // get the key
tnhnrl 16:3363b9f14913 299 userInput = pc().getc();
tnhnrl 16:3363b9f14913 300
tnhnrl 16:3363b9f14913 301 // check command against desired control buttons
tnhnrl 16:3363b9f14913 302 // change state
tnhnrl 16:3363b9f14913 303 if (userInput == 'D' or userInput == 'd') {
tnhnrl 16:3363b9f14913 304 return DIVE;
tnhnrl 16:3363b9f14913 305 }
tnhnrl 16:3363b9f14913 306 else if (userInput == 'N' or userInput == 'n') {
tnhnrl 16:3363b9f14913 307 return FIND_NEUTRAL;
tnhnrl 16:3363b9f14913 308 }
tnhnrl 16:3363b9f14913 309 else if (userInput == 'R' or userInput == 'r') {
tnhnrl 16:3363b9f14913 310 return RISE;
tnhnrl 16:3363b9f14913 311 }
tnhnrl 16:3363b9f14913 312 else if (userInput == 'L' or userInput == 'l') {
tnhnrl 16:3363b9f14913 313 return FLOAT_LEVEL;
tnhnrl 16:3363b9f14913 314 }
tnhnrl 16:3363b9f14913 315 else if (userInput == 'B' or userInput == 'b') {
tnhnrl 16:3363b9f14913 316 return FLOAT_BROADCAST;
tnhnrl 16:3363b9f14913 317 }
tnhnrl 16:3363b9f14913 318 else if (userInput == 'E' or userInput == 'e') {
tnhnrl 16:3363b9f14913 319 return EMERGENCY_CLIMB;
tnhnrl 16:3363b9f14913 320 }
tnhnrl 16:3363b9f14913 321 else if (userInput == 'H' or userInput == 'h') {
tnhnrl 16:3363b9f14913 322 pc().printf("running homing procedure\r\n");
tnhnrl 16:3363b9f14913 323 bce().unpause(); bce().homePiston(); bce().pause();
tnhnrl 16:3363b9f14913 324 batt().unpause(); batt().homePiston(); batt().pause();
tnhnrl 16:3363b9f14913 325 return SIT_IDLE;
tnhnrl 16:3363b9f14913 326 }
tnhnrl 16:3363b9f14913 327 else if (userInput == 'T' or userInput == 't') {
tnhnrl 16:3363b9f14913 328 pc().printf("taring depth sensor\r\n");
tnhnrl 16:3363b9f14913 329 pc().printf("Pre-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
tnhnrl 16:3363b9f14913 330 wait(0.1);
tnhnrl 16:3363b9f14913 331 depth().tare(); // tares to ambient (do on surface)
tnhnrl 16:3363b9f14913 332 pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt());
tnhnrl 16:3363b9f14913 333 }
tnhnrl 16:3363b9f14913 334
tnhnrl 16:3363b9f14913 335 else if (userInput == '[' or userInput == '{') {
tnhnrl 16:3363b9f14913 336 depthLoop().setOutputOffset(depthLoop().getOutputOffset() - 1); // decrease the bce neutral setpoint
tnhnrl 16:3363b9f14913 337 pc().printf("Adjusting bce neutral position. new: %3.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 16:3363b9f14913 338 }
tnhnrl 16:3363b9f14913 339 else if (userInput == ']' or userInput == '}') {
tnhnrl 16:3363b9f14913 340 depthLoop().setOutputOffset(depthLoop().getOutputOffset() + 1); // increase the bce neutral setpoint
tnhnrl 16:3363b9f14913 341 pc().printf("Adjusting bce neutral position. new: %3.1f\r\n",depthLoop().getOutputOffset());
tnhnrl 16:3363b9f14913 342 }
tnhnrl 16:3363b9f14913 343 else if (userInput == '<' or userInput == ',') {
tnhnrl 16:3363b9f14913 344 pitchLoop().setOutputOffset(pitchLoop().getOutputOffset() - 1); // decrease the batt neutral setpoint
tnhnrl 16:3363b9f14913 345 pc().printf("Adjusting batt neutral position. new: %3.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 16:3363b9f14913 346 }
tnhnrl 16:3363b9f14913 347 else if (userInput == '>' or userInput == '.') {
tnhnrl 16:3363b9f14913 348 pitchLoop().setOutputOffset(pitchLoop().getOutputOffset() + 1); // increase the batt neutral setpoint
tnhnrl 16:3363b9f14913 349 pc().printf("Adjusting batt neutral position. new: %3.1f\r\n",pitchLoop().getOutputOffset());
tnhnrl 16:3363b9f14913 350 }
tnhnrl 16:3363b9f14913 351
tnhnrl 16:3363b9f14913 352 else if (userInput == '?') {
tnhnrl 16:3363b9f14913 353 pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
tnhnrl 16:3363b9f14913 354 wait(0.5);
tnhnrl 16:3363b9f14913 355 mbed_reset();
tnhnrl 16:3363b9f14913 356 }
tnhnrl 16:3363b9f14913 357
tnhnrl 16:3363b9f14913 358 // change settings
tnhnrl 16:3363b9f14913 359 else if (userInput == 'Q' or userInput == 'q') {
tnhnrl 16:3363b9f14913 360 pitchCommand -= 0.5; //decrement the pitch setpoint
tnhnrl 16:3363b9f14913 361 pitchLoop().setCommand(pitchCommand);
tnhnrl 16:3363b9f14913 362 pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand());
tnhnrl 16:3363b9f14913 363 }
tnhnrl 16:3363b9f14913 364 else if (userInput == 'W' or userInput == 'w') {
tnhnrl 16:3363b9f14913 365 pitchCommand += 0.5; //increment the pitch setpoint
tnhnrl 16:3363b9f14913 366 pitchLoop().setCommand(pitchCommand);
tnhnrl 16:3363b9f14913 367 pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand());
tnhnrl 16:3363b9f14913 368 }
tnhnrl 16:3363b9f14913 369 else if (userInput == 'A' or userInput == 'a') {
tnhnrl 16:3363b9f14913 370 depthCommand -= 0.5; //decrement the depth setpoint
tnhnrl 16:3363b9f14913 371 depthLoop().setCommand(depthCommand);
tnhnrl 16:3363b9f14913 372 pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand());
tnhnrl 16:3363b9f14913 373 }
tnhnrl 16:3363b9f14913 374 else if (userInput == 'S' or userInput == 's') {
tnhnrl 16:3363b9f14913 375 depthCommand += 0.5; //increment the depth setpoint
tnhnrl 16:3363b9f14913 376 depthLoop().setCommand(depthCommand);
tnhnrl 16:3363b9f14913 377 pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
tnhnrl 16:3363b9f14913 378 }
tnhnrl 16:3363b9f14913 379 else if (userInput == '-') {
tnhnrl 16:3363b9f14913 380 timeout -= 10.0; //decrement the timeout
tnhnrl 16:3363b9f14913 381 pc().printf(">>> timeout decreased: %d\r\n", timeout);
tnhnrl 16:3363b9f14913 382 }
tnhnrl 16:3363b9f14913 383 else if (userInput == '=' or userInput == '+') {
tnhnrl 16:3363b9f14913 384 timeout += 10.0; //increment the timeout
tnhnrl 16:3363b9f14913 385 pc().printf(">>> timeout increased: %d\r\n", timeout);
tnhnrl 16:3363b9f14913 386 }
tnhnrl 16:3363b9f14913 387
tnhnrl 16:3363b9f14913 388 // add keyboard commands to move the neutral zero offsets, both bce and batt
tnhnrl 16:3363b9f14913 389
tnhnrl 16:3363b9f14913 390 // go to sub-menus for the PID gains (this is blocking)
tnhnrl 16:3363b9f14913 391 else if (userInput == '1') {
tnhnrl 16:3363b9f14913 392 keyboard_menu_BCE_PID_settings();
tnhnrl 16:3363b9f14913 393 }
tnhnrl 16:3363b9f14913 394 else if (userInput == '2') {
tnhnrl 16:3363b9f14913 395 keyboard_menu_BATT_PID_settings();
tnhnrl 16:3363b9f14913 396 }
tnhnrl 16:3363b9f14913 397 else if (userInput == '3') {
tnhnrl 16:3363b9f14913 398 keyboard_menu_DEPTH_PID_settings();
tnhnrl 16:3363b9f14913 399 }
tnhnrl 16:3363b9f14913 400 else if (userInput == '4') {
tnhnrl 16:3363b9f14913 401 keyboard_menu_PITCH_PID_settings();
tnhnrl 16:3363b9f14913 402 }
tnhnrl 16:3363b9f14913 403
tnhnrl 16:3363b9f14913 404 else if (userInput == 'C' or userInput == 'c') {
tnhnrl 16:3363b9f14913 405 pc().printf("depth: %3.1f\r\n",depthLoop().getPosition());
tnhnrl 16:3363b9f14913 406 pc().printf("pitch: %3.1f\r\n",imu().getPitch());
tnhnrl 16:3363b9f14913 407 pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
tnhnrl 16:3363b9f14913 408 pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
tnhnrl 16:3363b9f14913 409 pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
tnhnrl 16:3363b9f14913 410 pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
tnhnrl 16:3363b9f14913 411 pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
tnhnrl 16:3363b9f14913 412 pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
tnhnrl 16:3363b9f14913 413 }
tnhnrl 16:3363b9f14913 414 else {
tnhnrl 16:3363b9f14913 415 return (-1);
tnhnrl 16:3363b9f14913 416 }
tnhnrl 16:3363b9f14913 417 }
tnhnrl 16:3363b9f14913 418 return (-1);
tnhnrl 16:3363b9f14913 419 }
tnhnrl 16:3363b9f14913 420
tnhnrl 16:3363b9f14913 421 void StateMachine::keyboard_menu_BCE_PID_settings() {
tnhnrl 16:3363b9f14913 422 char PID_key;
tnhnrl 16:3363b9f14913 423 float gain_step_size = 0.01; // modify this to change gain step size
tnhnrl 16:3363b9f14913 424 float KP = bce().getControllerP(); // load current value
tnhnrl 16:3363b9f14913 425 float KI = bce().getControllerI(); // load current global value
tnhnrl 16:3363b9f14913 426 float KD = bce().getControllerD(); // load current global value
tnhnrl 16:3363b9f14913 427
tnhnrl 16:3363b9f14913 428 // show the menu
tnhnrl 16:3363b9f14913 429 pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
tnhnrl 16:3363b9f14913 430 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
tnhnrl 16:3363b9f14913 431 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
tnhnrl 16:3363b9f14913 432 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());
tnhnrl 16:3363b9f14913 433
tnhnrl 16:3363b9f14913 434 // handle the key presses
tnhnrl 16:3363b9f14913 435 while(1) {
tnhnrl 16:3363b9f14913 436 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 437 if (pc().readable()) {
tnhnrl 16:3363b9f14913 438 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 439 }
tnhnrl 16:3363b9f14913 440 else {
tnhnrl 16:3363b9f14913 441 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 442 }
tnhnrl 16:3363b9f14913 443
tnhnrl 16:3363b9f14913 444 // handle the user's key input
tnhnrl 16:3363b9f14913 445 if (PID_key == '-') {
tnhnrl 16:3363b9f14913 446 KP -= gain_step_size;
tnhnrl 16:3363b9f14913 447 pc().printf("P gain: %0.5f \r\n", KP);
tnhnrl 16:3363b9f14913 448 }
tnhnrl 16:3363b9f14913 449 else if (PID_key == '=') {
tnhnrl 16:3363b9f14913 450 KP += gain_step_size;
tnhnrl 16:3363b9f14913 451 pc().printf("P gain: %0.5f \r\n", KP);
tnhnrl 16:3363b9f14913 452 }
tnhnrl 16:3363b9f14913 453 else if (PID_key == '[') {
tnhnrl 16:3363b9f14913 454 KI -= gain_step_size;
tnhnrl 16:3363b9f14913 455 pc().printf("I gain: %0.5f \r\n", KI);
tnhnrl 16:3363b9f14913 456 }
tnhnrl 16:3363b9f14913 457 else if (PID_key == ']') {
tnhnrl 16:3363b9f14913 458 KI += gain_step_size;
tnhnrl 16:3363b9f14913 459 pc().printf("I gain: %0.5f \r\n", KI);
tnhnrl 16:3363b9f14913 460 }
tnhnrl 16:3363b9f14913 461 else if (PID_key == ';') {
tnhnrl 16:3363b9f14913 462 KD -= gain_step_size;
tnhnrl 16:3363b9f14913 463 pc().printf("D gain: %0.5f \r\n", KD);
tnhnrl 16:3363b9f14913 464 }
tnhnrl 16:3363b9f14913 465 else if (PID_key == '\'') {
tnhnrl 16:3363b9f14913 466 KD += gain_step_size;
tnhnrl 16:3363b9f14913 467 pc().printf("D gain: %0.5f \r\n", KD);
tnhnrl 16:3363b9f14913 468 }
tnhnrl 16:3363b9f14913 469 else if (PID_key == 'S') { // user wants to save these modified values
tnhnrl 16:3363b9f14913 470 // set values
tnhnrl 16:3363b9f14913 471 bce().setControllerP(KP);
tnhnrl 16:3363b9f14913 472 bce().setControllerI(KI);
tnhnrl 16:3363b9f14913 473 bce().setControllerD(KD);
tnhnrl 16:3363b9f14913 474
tnhnrl 16:3363b9f14913 475 // save into "PID.cfg"
tnhnrl 16:3363b9f14913 476 //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);
tnhnrl 16:3363b9f14913 477 break; //exit the while loop
tnhnrl 16:3363b9f14913 478 }
tnhnrl 16:3363b9f14913 479 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 480 break; //exit the while loop
tnhnrl 16:3363b9f14913 481 }
tnhnrl 16:3363b9f14913 482 else {
tnhnrl 16:3363b9f14913 483 pc().printf("\n\rThis key does nothing here. ");
tnhnrl 16:3363b9f14913 484 }
tnhnrl 16:3363b9f14913 485 }
tnhnrl 16:3363b9f14913 486 }
tnhnrl 16:3363b9f14913 487
tnhnrl 16:3363b9f14913 488 void StateMachine::keyboard_menu_BATT_PID_settings() {
tnhnrl 16:3363b9f14913 489 char PID_key;
tnhnrl 16:3363b9f14913 490 float gain_step_size = 0.01; // modify this to change gain step size
tnhnrl 16:3363b9f14913 491 float KP = batt().getControllerP(); // load current global value
tnhnrl 16:3363b9f14913 492 float KI = batt().getControllerI(); // load current global value
tnhnrl 16:3363b9f14913 493 float KD = batt().getControllerD(); // load current global value
tnhnrl 16:3363b9f14913 494
tnhnrl 16:3363b9f14913 495 // print the menu
tnhnrl 16:3363b9f14913 496 pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
tnhnrl 16:3363b9f14913 497 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
tnhnrl 16:3363b9f14913 498 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
tnhnrl 16:3363b9f14913 499 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());
tnhnrl 16:3363b9f14913 500
tnhnrl 16:3363b9f14913 501 // handle the key presses
tnhnrl 16:3363b9f14913 502 while(1) {
tnhnrl 16:3363b9f14913 503 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 504 if (pc().readable()) {
tnhnrl 16:3363b9f14913 505 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 506 }
tnhnrl 16:3363b9f14913 507 else {
tnhnrl 16:3363b9f14913 508 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 509 }
tnhnrl 16:3363b9f14913 510
tnhnrl 16:3363b9f14913 511 // handle the user's key input
tnhnrl 16:3363b9f14913 512 if (PID_key == '-') {
tnhnrl 16:3363b9f14913 513 KP -= gain_step_size;
tnhnrl 16:3363b9f14913 514 pc().printf("\rP gain: %0.5f ", KP);
tnhnrl 16:3363b9f14913 515 }
tnhnrl 16:3363b9f14913 516 else if (PID_key == '=') {
tnhnrl 16:3363b9f14913 517 KP += gain_step_size;
tnhnrl 16:3363b9f14913 518 pc().printf("\rP gain: %0.5f ", KP);
tnhnrl 16:3363b9f14913 519 }
tnhnrl 16:3363b9f14913 520 else if (PID_key == '[') {
tnhnrl 16:3363b9f14913 521 KI -= gain_step_size;
tnhnrl 16:3363b9f14913 522 pc().printf("\rI gain: %0.5f ", KI);
tnhnrl 16:3363b9f14913 523 }
tnhnrl 16:3363b9f14913 524 else if (PID_key == ']') {
tnhnrl 16:3363b9f14913 525 KI += gain_step_size;
tnhnrl 16:3363b9f14913 526 pc().printf("\rI gain: %0.5f ", KI);
tnhnrl 16:3363b9f14913 527 }
tnhnrl 16:3363b9f14913 528 else if (PID_key == ';') {
tnhnrl 16:3363b9f14913 529 KD -= gain_step_size;
tnhnrl 16:3363b9f14913 530 pc().printf("\rD gain: %0.5f ", KD);
tnhnrl 16:3363b9f14913 531 }
tnhnrl 16:3363b9f14913 532 else if (PID_key == '\'') {
tnhnrl 16:3363b9f14913 533 KD += gain_step_size;
tnhnrl 16:3363b9f14913 534 pc().printf("\rD gain: %0.5f ", KD);
tnhnrl 16:3363b9f14913 535 }
tnhnrl 16:3363b9f14913 536 else if (PID_key == 'S') { // user wants to save the modified values
tnhnrl 16:3363b9f14913 537 // set global values
tnhnrl 16:3363b9f14913 538 batt().setControllerP(KP);
tnhnrl 16:3363b9f14913 539 batt().setControllerI(KI);
tnhnrl 16:3363b9f14913 540 batt().setControllerD(KD);
tnhnrl 16:3363b9f14913 541
tnhnrl 16:3363b9f14913 542 // save to "PID.cfg" file
tnhnrl 16:3363b9f14913 543 //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);
tnhnrl 16:3363b9f14913 544 break; //exit the while loop
tnhnrl 16:3363b9f14913 545 }
tnhnrl 16:3363b9f14913 546 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 547 break; //exit the while loop
tnhnrl 16:3363b9f14913 548 }
tnhnrl 16:3363b9f14913 549 else {
tnhnrl 16:3363b9f14913 550 pc().printf("This key does nothing here.\r");
tnhnrl 16:3363b9f14913 551 }
tnhnrl 16:3363b9f14913 552 }
tnhnrl 16:3363b9f14913 553 }
tnhnrl 16:3363b9f14913 554
tnhnrl 16:3363b9f14913 555 void StateMachine::keyboard_menu_DEPTH_PID_settings() {
tnhnrl 16:3363b9f14913 556 char PID_key;
tnhnrl 16:3363b9f14913 557 float gain_step_size = 0.01; // modify this to change gain step size
tnhnrl 16:3363b9f14913 558 float KP = depthLoop().getControllerP(); // load current global value
tnhnrl 16:3363b9f14913 559 float KI = depthLoop().getControllerI(); // load current global value
tnhnrl 16:3363b9f14913 560 float KD = depthLoop().getControllerD(); // load current global value
tnhnrl 16:3363b9f14913 561
tnhnrl 16:3363b9f14913 562 // show the menu
tnhnrl 16:3363b9f14913 563 pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
tnhnrl 16:3363b9f14913 564 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
tnhnrl 16:3363b9f14913 565 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\n\n\r");
tnhnrl 16:3363b9f14913 566 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());
tnhnrl 16:3363b9f14913 567
tnhnrl 16:3363b9f14913 568 // handle the key presses
tnhnrl 16:3363b9f14913 569 while(1) {
tnhnrl 16:3363b9f14913 570 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 571 if (pc().readable()) {
tnhnrl 16:3363b9f14913 572 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 573 }
tnhnrl 16:3363b9f14913 574 else {
tnhnrl 16:3363b9f14913 575 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 576 }
tnhnrl 16:3363b9f14913 577
tnhnrl 16:3363b9f14913 578 // handle the user's key input
tnhnrl 16:3363b9f14913 579 if (PID_key == '-') {
tnhnrl 16:3363b9f14913 580 KP -= gain_step_size;
tnhnrl 16:3363b9f14913 581 pc().printf("P gain: %0.5f \r\n", KP);
tnhnrl 16:3363b9f14913 582 }
tnhnrl 16:3363b9f14913 583 else if (PID_key == '=') {
tnhnrl 16:3363b9f14913 584 KP += gain_step_size;
tnhnrl 16:3363b9f14913 585 pc().printf("P gain: %0.5f \r\n", KP);
tnhnrl 16:3363b9f14913 586 }
tnhnrl 16:3363b9f14913 587 else if (PID_key == '[') {
tnhnrl 16:3363b9f14913 588 KI -= gain_step_size;
tnhnrl 16:3363b9f14913 589 pc().printf("I gain: %0.5f \r\n", KI);
tnhnrl 16:3363b9f14913 590 }
tnhnrl 16:3363b9f14913 591 else if (PID_key == ']') {
tnhnrl 16:3363b9f14913 592 KI += gain_step_size;
tnhnrl 16:3363b9f14913 593 pc().printf("I gain: %0.5f \r\n", KI);
tnhnrl 16:3363b9f14913 594 }
tnhnrl 16:3363b9f14913 595 else if (PID_key == ';') {
tnhnrl 16:3363b9f14913 596 KD -= gain_step_size;
tnhnrl 16:3363b9f14913 597 pc().printf("D gain: %0.5f \r\n", KD);
tnhnrl 16:3363b9f14913 598 }
tnhnrl 16:3363b9f14913 599 else if (PID_key == '\'') {
tnhnrl 16:3363b9f14913 600 KD += gain_step_size;
tnhnrl 16:3363b9f14913 601 pc().printf("D gain: %0.5f \r\n", KD);
tnhnrl 16:3363b9f14913 602 }
tnhnrl 16:3363b9f14913 603 else if (PID_key == 'S') { // user wants to save these settings
tnhnrl 16:3363b9f14913 604 // set global values
tnhnrl 16:3363b9f14913 605 depthLoop().setControllerP(KP);
tnhnrl 16:3363b9f14913 606 depthLoop().setControllerI(KI);
tnhnrl 16:3363b9f14913 607 depthLoop().setControllerD(KD);
tnhnrl 16:3363b9f14913 608
tnhnrl 16:3363b9f14913 609 // save into "PID.cfg"
tnhnrl 16:3363b9f14913 610 //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);
tnhnrl 16:3363b9f14913 611 break; //exit the while loop
tnhnrl 16:3363b9f14913 612 }
tnhnrl 16:3363b9f14913 613 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 614 break; //exit the while loop
tnhnrl 16:3363b9f14913 615 }
tnhnrl 16:3363b9f14913 616 else {
tnhnrl 16:3363b9f14913 617 pc().printf("\n\rThis key does nothing here. ");
tnhnrl 16:3363b9f14913 618 }
tnhnrl 16:3363b9f14913 619 }
tnhnrl 16:3363b9f14913 620 }
tnhnrl 16:3363b9f14913 621
tnhnrl 16:3363b9f14913 622 void StateMachine::keyboard_menu_PITCH_PID_settings() {
tnhnrl 16:3363b9f14913 623 char PID_key;
tnhnrl 16:3363b9f14913 624 float gain_step_size = 0.01; // modify this to change gain step size
tnhnrl 16:3363b9f14913 625 float KP = pitchLoop().getControllerP(); // load current global value
tnhnrl 16:3363b9f14913 626 float KI = pitchLoop().getControllerI(); // load current global value
tnhnrl 16:3363b9f14913 627 float KD = pitchLoop().getControllerD(); // load current global value
tnhnrl 16:3363b9f14913 628
tnhnrl 16:3363b9f14913 629 // print the menu
tnhnrl 16:3363b9f14913 630 pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
tnhnrl 16:3363b9f14913 631 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
tnhnrl 16:3363b9f14913 632 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
tnhnrl 16:3363b9f14913 633 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());
tnhnrl 16:3363b9f14913 634
tnhnrl 16:3363b9f14913 635 // handle the key presses
tnhnrl 16:3363b9f14913 636 while(1) {
tnhnrl 16:3363b9f14913 637 // get the user's keystroke from either of the two inputs
tnhnrl 16:3363b9f14913 638 if (pc().readable()) {
tnhnrl 16:3363b9f14913 639 PID_key = pc().getc();
tnhnrl 16:3363b9f14913 640 }
tnhnrl 16:3363b9f14913 641 else {
tnhnrl 16:3363b9f14913 642 continue; // didn't get a user input, so keep waiting for it
tnhnrl 16:3363b9f14913 643 }
tnhnrl 16:3363b9f14913 644
tnhnrl 16:3363b9f14913 645 // handle the user's key input
tnhnrl 16:3363b9f14913 646 if (PID_key == '-') {
tnhnrl 16:3363b9f14913 647 KP -= gain_step_size;
tnhnrl 16:3363b9f14913 648 pc().printf("\rP gain: %0.5f ", KP);
tnhnrl 16:3363b9f14913 649 }
tnhnrl 16:3363b9f14913 650 else if (PID_key == '=') {
tnhnrl 16:3363b9f14913 651 KP += gain_step_size;
tnhnrl 16:3363b9f14913 652 pc().printf("\rP gain: %0.5f ", KP);
tnhnrl 16:3363b9f14913 653 }
tnhnrl 16:3363b9f14913 654 else if (PID_key == '[') {
tnhnrl 16:3363b9f14913 655 KI -= gain_step_size;
tnhnrl 16:3363b9f14913 656 pc().printf("\rI gain: %0.5f ", KI);
tnhnrl 16:3363b9f14913 657 }
tnhnrl 16:3363b9f14913 658 else if (PID_key == ']') {
tnhnrl 16:3363b9f14913 659 KI += gain_step_size;
tnhnrl 16:3363b9f14913 660 pc().printf("\rI gain: %0.5f ", KI);
tnhnrl 16:3363b9f14913 661 }
tnhnrl 16:3363b9f14913 662 else if (PID_key == ';') {
tnhnrl 16:3363b9f14913 663 KD -= gain_step_size;
tnhnrl 16:3363b9f14913 664 pc().printf("\rD gain: %0.5f ", KD);
tnhnrl 16:3363b9f14913 665 }
tnhnrl 16:3363b9f14913 666 else if (PID_key == '\'') {
tnhnrl 16:3363b9f14913 667 KD += gain_step_size;
tnhnrl 16:3363b9f14913 668 pc().printf("\rD gain: %0.5f ", KD);
tnhnrl 16:3363b9f14913 669 }
tnhnrl 16:3363b9f14913 670 else if (PID_key == 'S') { // user wants to save the modified values
tnhnrl 16:3363b9f14913 671 // set global values
tnhnrl 16:3363b9f14913 672 pitchLoop().setControllerP(KP);
tnhnrl 16:3363b9f14913 673 pitchLoop().setControllerI(KI);
tnhnrl 16:3363b9f14913 674 pitchLoop().setControllerD(KD);
tnhnrl 16:3363b9f14913 675
tnhnrl 16:3363b9f14913 676 //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);
tnhnrl 16:3363b9f14913 677 break; //exit the while loop
tnhnrl 16:3363b9f14913 678 }
tnhnrl 16:3363b9f14913 679 else if (PID_key == 'X') {
tnhnrl 16:3363b9f14913 680 break; //exit the while loop
tnhnrl 16:3363b9f14913 681 }
tnhnrl 16:3363b9f14913 682 else {
tnhnrl 16:3363b9f14913 683 pc().printf("This key does nothing here.\r");
tnhnrl 16:3363b9f14913 684 }
tnhnrl 16:3363b9f14913 685 }
tnhnrl 16:3363b9f14913 686 }
tnhnrl 16:3363b9f14913 687
tnhnrl 16:3363b9f14913 688 float StateMachine::getDepthCommand() {
tnhnrl 16:3363b9f14913 689 return depthCommand;
tnhnrl 16:3363b9f14913 690 }
tnhnrl 16:3363b9f14913 691
tnhnrl 16:3363b9f14913 692 float StateMachine::getPitchCommand() {
tnhnrl 16:3363b9f14913 693 return pitchCommand;
tnhnrl 16:3363b9f14913 694 }