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
Diff: StateMachine/StateMachine.cpp
- Revision:
- 16:3363b9f14913
- Child:
- 17:7c16b5671d0e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/StateMachine/StateMachine.cpp Mon Nov 06 22:57:56 2017 +0000 @@ -0,0 +1,694 @@ +#include "StateMachine.hpp" +#include "StaticDefs.hpp" + +StateMachine::StateMachine() { + timeout = 60; // generic timeout for every state, seconds + depthTolerance = 0.25; // depth tolerance for neutral finding exit critera + pitchTolerance = 1.0; // pitch angle tolerance for neutral finding exit criteria + bceFloatPosition = 300; // bce position for "float" states + battFloatPosition = 50; // batt position for "broadcast" state + + depthCommand = 3.5; // user keyboard depth + pitchCommand = -20.0; // user keyboard depth +} + +void StateMachine::runStateMachine() { + static int state = SIT_IDLE; // select starting state here + static bool isTimeoutRunning = false; // default timer to not running + + // finite state machine ... each state has at least one exit criteria + switch (state) { + case SIT_IDLE : + // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions + if (!isTimeoutRunning) { + showMenu(); + pc().printf("\r\n\nstate: SIT_IDLE\r\n"); + isTimeoutRunning = true; + + // what is active? + bce().pause(); + batt().pause(); + } + // how exit? + if (pc().readable()) { + state = KEYBOARD; + isTimeoutRunning = false; + } + break; + + case KEYBOARD : + pc().printf("\r\n\nstate: KEYBOARD\r\n"); + if (pc().readable()) { + state = keyboard(); // get new state command + if (state == -1) { // error, that wasn't a new state command + state = SIT_IDLE; + } + //pc().printf("new state is: %d \r\n",state); + } + break; + + case EMERGENCY_CLIMB : + // start local state timer and init any other one-shot actions + if (!isTimeoutRunning) { + pc().printf("\r\n\nstate: EMERGENCY_CLIMB\r\n"); + timer.reset(); // timer goes back to zero + timer.start(); // background timer starts running + isTimeoutRunning = true; + + // what needs to be started? + bce().unpause(); + batt().unpause(); + + // what is active? + bce().setPosition_mm(bce().getTravelLimit()); + batt().setPosition_mm(0.0); + } + // how exit? + if (timer > timeout) { + pc().printf("EC: timed out\r\n"); + state = FLOAT_LEVEL; + timer.reset(); + isTimeoutRunning = false; + } + else if (depthLoop().getPosition() < 0.2) { + pc().printf("EC: depth: %3.1f, cmd: 0.5 [%0.1f sec]\r",depthLoop().getPosition(), timer.read()); + state = FLOAT_LEVEL; + timer.reset(); + isTimeoutRunning = false; + } + break; + + case FIND_NEUTRAL : + // start local state timer and init any other one-shot actions + if (!isTimeoutRunning) { + pc().printf("\r\n\nstate: FIND_NEUTRAL\r\n"); + timer.reset(); // timer goes back to zero + timer.start(); // background timer starts running + isTimeoutRunning = true; + + // what needs to be started? + bce().unpause(); + batt().unpause(); + + // what is active? + depthLoop().setCommand(depthCommand); + pitchLoop().setCommand(0.0); + } + // how exit? + if (timer > timeout) { + pc().printf("FN: timed out\r\n"); + state = FLOAT_LEVEL; + timer.reset(); + isTimeoutRunning = false; + } + + //depth tolerance of 0.25 feet, pitch tolerance of 1.0 degree + else if ((abs(depthLoop().getPosition() - depthLoop().getCommand()) < depthTolerance) and + (abs(imu().getPitch() - pitchLoop().getCommand()) < pitchTolerance)) { + state = RISE; + timer.reset(); + isTimeoutRunning = false; + } + // what is active? + 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()); + bce().setPosition_mm(depthLoop().getOutput()); + batt().setPosition_mm(pitchLoop().getOutput()); + break; + + case DIVE : + // start local state timer and init any other one-shot actions + if (!isTimeoutRunning) { + pc().printf("\r\n\nstate: DIVE\r\n"); + timer.reset(); // timer goes back to zero + timer.start(); // background timer starts running + isTimeoutRunning = true; + + // what needs to be started? + bce().unpause(); + batt().unpause(); + + // what are the commands? + depthLoop().setCommand(depthCommand); + pitchLoop().setCommand(pitchCommand); + pc().printf("DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand()); + pc().printf("DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); + } + // how exit? + if (timer > timeout) { + pc().printf("DIVE: timed out\r\n"); + state = EMERGENCY_CLIMB; + timer.reset(); + isTimeoutRunning = false; + } + else if (depthLoop().getPosition() > depthLoop().getCommand()) { + pc().printf("DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); + state = RISE; + timer.reset(); + isTimeoutRunning = false; + } + // what is active? + 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()); + bce().setPosition_mm(depthLoop().getOutput()); + batt().setPosition_mm(pitchLoop().getOutput()); + break; + + case RISE : + // start local state timer and init any other one-shot actions + if (!isTimeoutRunning) { + pc().printf("\r\n\nstate: RISE\r\n"); + timer.reset(); // timer goes back to zero + timer.start(); // background timer starts running + isTimeoutRunning = true; + + // what needs to be started? + bce().unpause(); + batt().unpause(); + + // what are the commands? + depthLoop().setCommand(0.0); + pitchLoop().setCommand(-pitchCommand); + pc().printf("RISE: depth cmd: 0.0\r\n"); + pc().printf("RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand()); + } + // how exit? + if (timer > timeout) { + pc().printf("RISE: timed out\r\n"); + state = FLOAT_LEVEL; + timer.reset(); + isTimeoutRunning = false; + } + else if (depthLoop().getPosition() < depthLoop().getCommand()) { + pc().printf("RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand()); + state = FLOAT_LEVEL; + timer.reset(); + isTimeoutRunning = false; + } + 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()); + // what is active? + bce().setPosition_mm(depthLoop().getOutput()); + batt().setPosition_mm(pitchLoop().getOutput()); + break; + + case FLOAT_LEVEL : + // start local state timer and init any other one-shot actions + if (!isTimeoutRunning) { + pc().printf("\r\n\nstate: FLOAT_LEVEL\r\n"); + timer.reset(); // timer goes back to zero + timer.start(); // background timer starts running + isTimeoutRunning = true; + + // what needs to be started? + bce().unpause(); + batt().unpause(); + + // what are the commands + bce().setPosition_mm(bceFloatPosition); + pitchLoop().setCommand(0.0); + } + // how exit? + if (timer > timeout) { + pc().printf("FL: timed out\r\n"); + state = FLOAT_BROADCAST; + timer.reset(); + isTimeoutRunning = false; + } + else if (abs(imu().getPitch() - pitchLoop().getCommand()) < abs(pitchTolerance)) { + pc().printf("FL: pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), pitchTolerance); + state = FLOAT_BROADCAST; + timer.reset(); + isTimeoutRunning = false; + } + // what is active? + 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()); + batt().setPosition_mm(pitchLoop().getOutput()); + break; + + case FLOAT_BROADCAST : + // start local state timer and init any other one-shot actions + if (!isTimeoutRunning) { + pc().printf("\r\n\nstate: FLOAT_BROADCAST\r\n"); + timer.reset(); // timer goes back to zero + timer.start(); // background timer starts running + isTimeoutRunning = true; + + // what needs to be started? + bce().unpause(); + batt().unpause(); + + // what are the commands? + bce().setPosition_mm(bceFloatPosition); + batt().setPosition_mm(battFloatPosition); + } + // how exit? + if (timer > timeout) { + pc().printf("FB: timed out\r\n"); + state = SIT_IDLE; + timer.reset(); + isTimeoutRunning = false; + } + if ( (abs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and + (abs(batt().getPosition_mm() - batt().getSetPosition_mm()) < batt().getDeadband()) ) { + 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()); + state = SIT_IDLE; + timer.reset(); + isTimeoutRunning = false; + } + 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()); + break; + + default : + state = SIT_IDLE; + } +} + +// output the keyboard menu for user's reference +void StateMachine::showMenu() { + pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n"); + pc().printf(" N to find neutral\r\n"); + pc().printf(" D to initiate dive cycle\r\n"); + pc().printf(" R to initiate rise\r\n"); + pc().printf(" L to float level\r\n"); + pc().printf(" B to float at broadcast pitch\r\n"); + pc().printf(" E to initiate emergency climb\r\n"); + pc().printf(" H to run homing sequence on both BCE and Batt\r\n"); + pc().printf(" T to tare the depth sensor\r\n"); + pc().printf("[/] to change bce neutral position\r\n"); + pc().printf("</> to change batt neutral position\r\n"); + pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",pitchCommand); + pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",depthCommand); + //pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",pitchLoop().getCommand()); + //pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",depthLoop().getCommand()); + pc().printf("+/- to decrease/increase timeout: %d s\r\n",timeout); + pc().printf(" 1 BCE PID sub-menu\r\n"); + pc().printf(" 2 BATT PID sub-menu\r\n"); + pc().printf(" 3 Depth PID sub-menu\r\n"); + pc().printf(" 4 Pitch PID sub-menu\r\n"); + pc().printf(" C See sensor readings\r\n"); + pc().printf(" ? to reset mbed\r\n"); +} + +// keyboard currently handles a key at a time +// returns -1 if not a state command +// returns a positive number to command a new state +int StateMachine::keyboard() { + char userInput; + + // check keyboard and make settings changes as requested + if (pc().readable()) { + // get the key + userInput = pc().getc(); + + // check command against desired control buttons + // change state + if (userInput == 'D' or userInput == 'd') { + return DIVE; + } + else if (userInput == 'N' or userInput == 'n') { + return FIND_NEUTRAL; + } + else if (userInput == 'R' or userInput == 'r') { + return RISE; + } + else if (userInput == 'L' or userInput == 'l') { + return FLOAT_LEVEL; + } + else if (userInput == 'B' or userInput == 'b') { + return FLOAT_BROADCAST; + } + else if (userInput == 'E' or userInput == 'e') { + return EMERGENCY_CLIMB; + } + else if (userInput == 'H' or userInput == 'h') { + pc().printf("running homing procedure\r\n"); + bce().unpause(); bce().homePiston(); bce().pause(); + batt().unpause(); batt().homePiston(); batt().pause(); + return SIT_IDLE; + } + else if (userInput == 'T' or userInput == 't') { + pc().printf("taring depth sensor\r\n"); + pc().printf("Pre-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt()); + wait(0.1); + depth().tare(); // tares to ambient (do on surface) + pc().printf("Post-tare: press: %3.3f psi, depth: %3.3f ft\r\n", depth().getPsi(), depth().getDepthFt()); + } + + else if (userInput == '[' or userInput == '{') { + depthLoop().setOutputOffset(depthLoop().getOutputOffset() - 1); // decrease the bce neutral setpoint + pc().printf("Adjusting bce neutral position. new: %3.1f\r\n",depthLoop().getOutputOffset()); + } + else if (userInput == ']' or userInput == '}') { + depthLoop().setOutputOffset(depthLoop().getOutputOffset() + 1); // increase the bce neutral setpoint + pc().printf("Adjusting bce neutral position. new: %3.1f\r\n",depthLoop().getOutputOffset()); + } + else if (userInput == '<' or userInput == ',') { + pitchLoop().setOutputOffset(pitchLoop().getOutputOffset() - 1); // decrease the batt neutral setpoint + pc().printf("Adjusting batt neutral position. new: %3.1f\r\n",pitchLoop().getOutputOffset()); + } + else if (userInput == '>' or userInput == '.') { + pitchLoop().setOutputOffset(pitchLoop().getOutputOffset() + 1); // increase the batt neutral setpoint + pc().printf("Adjusting batt neutral position. new: %3.1f\r\n",pitchLoop().getOutputOffset()); + } + + else if (userInput == '?') { + pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n"); + wait(0.5); + mbed_reset(); + } + + // change settings + else if (userInput == 'Q' or userInput == 'q') { + pitchCommand -= 0.5; //decrement the pitch setpoint + pitchLoop().setCommand(pitchCommand); + pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand()); + } + else if (userInput == 'W' or userInput == 'w') { + pitchCommand += 0.5; //increment the pitch setpoint + pitchLoop().setCommand(pitchCommand); + pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand()); + } + else if (userInput == 'A' or userInput == 'a') { + depthCommand -= 0.5; //decrement the depth setpoint + depthLoop().setCommand(depthCommand); + pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand()); + } + else if (userInput == 'S' or userInput == 's') { + depthCommand += 0.5; //increment the depth setpoint + depthLoop().setCommand(depthCommand); + pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand()); + } + else if (userInput == '-') { + timeout -= 10.0; //decrement the timeout + pc().printf(">>> timeout decreased: %d\r\n", timeout); + } + else if (userInput == '=' or userInput == '+') { + timeout += 10.0; //increment the timeout + pc().printf(">>> timeout increased: %d\r\n", timeout); + } + + // add keyboard commands to move the neutral zero offsets, both bce and batt + + // go to sub-menus for the PID gains (this is blocking) + else if (userInput == '1') { + keyboard_menu_BCE_PID_settings(); + } + else if (userInput == '2') { + keyboard_menu_BATT_PID_settings(); + } + else if (userInput == '3') { + keyboard_menu_DEPTH_PID_settings(); + } + else if (userInput == '4') { + keyboard_menu_PITCH_PID_settings(); + } + + else if (userInput == 'C' or userInput == 'c') { + pc().printf("depth: %3.1f\r\n",depthLoop().getPosition()); + pc().printf("pitch: %3.1f\r\n",imu().getPitch()); + pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm()); + pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm()); + pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm()); + pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm()); + pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand()); + pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand()); + } + else { + return (-1); + } + } + return (-1); +} + +void StateMachine::keyboard_menu_BCE_PID_settings() { + char PID_key; + float gain_step_size = 0.01; // modify this to change gain step size + float KP = bce().getControllerP(); // load current value + float KI = bce().getControllerI(); // load current global value + float KD = bce().getControllerD(); // load current global value + + // show the menu + pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)"); + pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'"); + pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r"); + 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()); + + // handle the key presses + while(1) { + // get the user's keystroke from either of the two inputs + if (pc().readable()) { + PID_key = pc().getc(); + } + else { + continue; // didn't get a user input, so keep waiting for it + } + + // handle the user's key input + if (PID_key == '-') { + KP -= gain_step_size; + pc().printf("P gain: %0.5f \r\n", KP); + } + else if (PID_key == '=') { + KP += gain_step_size; + pc().printf("P gain: %0.5f \r\n", KP); + } + else if (PID_key == '[') { + KI -= gain_step_size; + pc().printf("I gain: %0.5f \r\n", KI); + } + else if (PID_key == ']') { + KI += gain_step_size; + pc().printf("I gain: %0.5f \r\n", KI); + } + else if (PID_key == ';') { + KD -= gain_step_size; + pc().printf("D gain: %0.5f \r\n", KD); + } + else if (PID_key == '\'') { + KD += gain_step_size; + pc().printf("D gain: %0.5f \r\n", KD); + } + else if (PID_key == 'S') { // user wants to save these modified values + // set values + bce().setControllerP(KP); + bce().setControllerI(KI); + bce().setControllerD(KD); + + // save into "PID.cfg" + //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); + break; //exit the while loop + } + else if (PID_key == 'X') { + break; //exit the while loop + } + else { + pc().printf("\n\rThis key does nothing here. "); + } + } +} + +void StateMachine::keyboard_menu_BATT_PID_settings() { + char PID_key; + float gain_step_size = 0.01; // modify this to change gain step size + float KP = batt().getControllerP(); // load current global value + float KI = batt().getControllerI(); // load current global value + float KD = batt().getControllerD(); // load current global value + + // print the menu + pc().printf("\n\r2: Battery Motor PID gain settings (MENU)"); + pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'"); + pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r"); + 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()); + + // handle the key presses + while(1) { + // get the user's keystroke from either of the two inputs + if (pc().readable()) { + PID_key = pc().getc(); + } + else { + continue; // didn't get a user input, so keep waiting for it + } + + // handle the user's key input + if (PID_key == '-') { + KP -= gain_step_size; + pc().printf("\rP gain: %0.5f ", KP); + } + else if (PID_key == '=') { + KP += gain_step_size; + pc().printf("\rP gain: %0.5f ", KP); + } + else if (PID_key == '[') { + KI -= gain_step_size; + pc().printf("\rI gain: %0.5f ", KI); + } + else if (PID_key == ']') { + KI += gain_step_size; + pc().printf("\rI gain: %0.5f ", KI); + } + else if (PID_key == ';') { + KD -= gain_step_size; + pc().printf("\rD gain: %0.5f ", KD); + } + else if (PID_key == '\'') { + KD += gain_step_size; + pc().printf("\rD gain: %0.5f ", KD); + } + else if (PID_key == 'S') { // user wants to save the modified values + // set global values + batt().setControllerP(KP); + batt().setControllerI(KI); + batt().setControllerD(KD); + + // save to "PID.cfg" file + //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); + break; //exit the while loop + } + else if (PID_key == 'X') { + break; //exit the while loop + } + else { + pc().printf("This key does nothing here.\r"); + } + } +} + +void StateMachine::keyboard_menu_DEPTH_PID_settings() { + char PID_key; + float gain_step_size = 0.01; // modify this to change gain step size + float KP = depthLoop().getControllerP(); // load current global value + float KI = depthLoop().getControllerI(); // load current global value + float KD = depthLoop().getControllerD(); // load current global value + + // show the menu + pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)"); + pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'"); + pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\n\n\r"); + 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()); + + // handle the key presses + while(1) { + // get the user's keystroke from either of the two inputs + if (pc().readable()) { + PID_key = pc().getc(); + } + else { + continue; // didn't get a user input, so keep waiting for it + } + + // handle the user's key input + if (PID_key == '-') { + KP -= gain_step_size; + pc().printf("P gain: %0.5f \r\n", KP); + } + else if (PID_key == '=') { + KP += gain_step_size; + pc().printf("P gain: %0.5f \r\n", KP); + } + else if (PID_key == '[') { + KI -= gain_step_size; + pc().printf("I gain: %0.5f \r\n", KI); + } + else if (PID_key == ']') { + KI += gain_step_size; + pc().printf("I gain: %0.5f \r\n", KI); + } + else if (PID_key == ';') { + KD -= gain_step_size; + pc().printf("D gain: %0.5f \r\n", KD); + } + else if (PID_key == '\'') { + KD += gain_step_size; + pc().printf("D gain: %0.5f \r\n", KD); + } + else if (PID_key == 'S') { // user wants to save these settings + // set global values + depthLoop().setControllerP(KP); + depthLoop().setControllerI(KI); + depthLoop().setControllerD(KD); + + // save into "PID.cfg" + //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); + break; //exit the while loop + } + else if (PID_key == 'X') { + break; //exit the while loop + } + else { + pc().printf("\n\rThis key does nothing here. "); + } + } +} + +void StateMachine::keyboard_menu_PITCH_PID_settings() { + char PID_key; + float gain_step_size = 0.01; // modify this to change gain step size + float KP = pitchLoop().getControllerP(); // load current global value + float KI = pitchLoop().getControllerI(); // load current global value + float KD = pitchLoop().getControllerD(); // load current global value + + // print the menu + pc().printf("\n\r2: Battery Motor PID gain settings (MENU)"); + pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'"); + pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r"); + 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()); + + // handle the key presses + while(1) { + // get the user's keystroke from either of the two inputs + if (pc().readable()) { + PID_key = pc().getc(); + } + else { + continue; // didn't get a user input, so keep waiting for it + } + + // handle the user's key input + if (PID_key == '-') { + KP -= gain_step_size; + pc().printf("\rP gain: %0.5f ", KP); + } + else if (PID_key == '=') { + KP += gain_step_size; + pc().printf("\rP gain: %0.5f ", KP); + } + else if (PID_key == '[') { + KI -= gain_step_size; + pc().printf("\rI gain: %0.5f ", KI); + } + else if (PID_key == ']') { + KI += gain_step_size; + pc().printf("\rI gain: %0.5f ", KI); + } + else if (PID_key == ';') { + KD -= gain_step_size; + pc().printf("\rD gain: %0.5f ", KD); + } + else if (PID_key == '\'') { + KD += gain_step_size; + pc().printf("\rD gain: %0.5f ", KD); + } + else if (PID_key == 'S') { // user wants to save the modified values + // set global values + pitchLoop().setControllerP(KP); + pitchLoop().setControllerI(KI); + pitchLoop().setControllerD(KD); + + //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); + break; //exit the while loop + } + else if (PID_key == 'X') { + break; //exit the while loop + } + else { + pc().printf("This key does nothing here.\r"); + } + } +} + +float StateMachine::getDepthCommand() { + return depthCommand; +} + +float StateMachine::getPitchCommand() { + return pitchCommand; +} \ No newline at end of file