Dependencies: mbed MODSERIAL FATFileSystem
Diff: StateMachine/StateMachine.cpp
- Revision:
- 63:6cb0405fc6e6
- Parent:
- 58:94b7fd55185e
- Child:
- 64:a8939bc127ab
--- a/StateMachine/StateMachine.cpp Mon Jun 18 14:45:37 2018 +0000
+++ b/StateMachine/StateMachine.cpp Mon Jun 18 21:02:55 2018 +0000
@@ -709,6 +709,9 @@
//reset multi-dive sequence to start
_multi_dive_counter = 0;
+
+ //Reload the dive sequence on exit
+ sequenceController().loadSequence();
}
else if (depthLoop().getPosition() < 0.5) { // depth is less than 0.5 (zero is surface level)
pc().printf("MULTI-RISE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
@@ -725,11 +728,14 @@
//check if this is the end of the dive sequence
//CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (FLOAT_BROADCAST)
if (currentStateStruct.state == FLOAT_BROADCAST) {
+ //Reload the dive sequence on exit
+ sequenceController().loadSequence();
+
_state = FLOAT_BROADCAST;
}
else
- _state = MULTI_DIVE;
+ _state = MULTI_DIVE; //Note: need to test if this else statement is necessary
//have to stop this with the _multi_dive_counter variable!
}
@@ -744,9 +750,9 @@
break;
- case TRANSMIT_LOG:
- if (!_isTimeoutRunning) {
- pc().printf("\r\n\nstate: TRANSMIT_LOG\r\n");
+ case TRANSMIT_MBED_LOG:
+ if (!_isTimeoutRunning) {
+ pc().printf("\r\n\nstate: TRANSMIT_MBED_LOG\r\n");
timer.reset(); // timer goes back to zero
timer.start(); // background timer starts running
_isTimeoutRunning = true;
@@ -754,39 +760,43 @@
mbedLogger().getNumberOfPacketsInCurrentLog(); //open the file, read the number of lines in the log file
transmit_packet_number = mbedLogger().getNumberOfPackets();
- //pc().printf("getNumberOfPacketsInCurrentLog is %d\r\n", transmit_packet_number);
+ //set the timeout internally (integer value)!
+ _timeout = mbedLogger().getLogSize(); //works after current log function above
+ _timeout = _timeout - (_timeout % 10) + 10; //remove the last digit and keep it as multiples of 10 (add a buffer of 10 seconds just in case it's below 10)
+
+ //STATE
+ //TRANSMIT_MBED_LOG
}
if (timer.read() > _timeout) {
- pc().printf("\r\nTRANSMIT_LOG: timed out!\r\n");
+ mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
+ pc().printf("\r\TRANSMIT_MBED_LOG: timed out!\r\n");
+
+ //STATE
_state = SIT_IDLE;
+
+ timer.reset();
+ _isTimeoutRunning = false;
+
+ //send a command to the Python GUI to stop looking for data
+ //SEND COMMAND WHEN TRANSMISSION ENDS PREMATURELY
+ pc().printf("%c%c%c%c",0x15,0x15,0x15,0x15);
+ }
+
+ //IF THIS IS ZERO
+ if (mbedLogger().currentPacketNumber() > transmit_packet_number) {
+ pc().printf("mbedLogger().currentPacketNumber() > transmit_packet_number");
+
+ //STATE
+ _state = SIT_IDLE;
+
timer.reset();
_isTimeoutRunning = false;
mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
}
- //IF THIS IS ZERO
- if (mbedLogger().currentPacketNumber() > transmit_packet_number) {
- pc().printf("mbedLogger().currentPacketNumber() > transmit_packet_number");
- _state = SIT_IDLE;
- timer.reset();
- _isTimeoutRunning = false;
-
- mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
- }
-
- if (mbedLogger().isTransmissionComplete()) {
- pc().printf("StateMachine isTransmissionComplete (true)\r\n");
- _state = SIT_IDLE;
- timer.reset();
- _isTimeoutRunning = false;
-
- mbedLogger().closeLogFile(); //on timeout close the log file that was opened for reading
- }
-
-
- // what is active? (no hardware should be active)
+ //What is active?
mbedLogger().readTransmitPacketOneChar(); //led2 shows you pc readable
break;
@@ -837,7 +847,7 @@
// output the keyboard menu for user's reference
void StateMachine::showSimpleMenu() {
pc().printf("\r\r\n\nSIMPLE KEYBOARD MENU (06/15/2018):\r\r\n"); //make sure depth sensor tares itself on startup
- pc().printf(" Neutral Position BCE: %0.1f BMM: %01.f \r\n", _neutral_bce_pos_mm, _neutral_batt_pos_mm);
+ pc().printf(" Neutral Positions BCE: %0.1f BMM: %0.1f \r\n", _neutral_bce_pos_mm, _neutral_batt_pos_mm);
pc().printf(" V to POSITION DIVE (initiate motor position-based dive cycle)\r\n");
pc().printf(" J to float level\r\n");
@@ -1181,7 +1191,7 @@
mbedLogger().printMbedDirectory(); //print all log files to the screen
}
else if (user_input == 'O') {
- _keyboard_state = TRANSMIT_LOG; //Transmit data (work in progress)
+ _keyboard_state = TRANSMIT_MBED_LOG; //Transmit data (work in progress)
}
else if (user_input == 'I') {
mbedLogger().receiveMissionDataWithTicker(); //receive sequence.txt files
@@ -1453,7 +1463,7 @@
mbedLogger().printCurrentLogFile(); //print the current log file to the screen
}
else if (user_input == 'O') {
- _keyboard_state = TRANSMIT_LOG; //Transmit data (work in progress)
+ _keyboard_state = TRANSMIT_MBED_LOG; //Transmit data (work in progress)
}
else if (user_input == '~') {
pc().printf("ERASING MBED LOG FILE\r\n");
@@ -1711,7 +1721,7 @@
pc().printf("\n\r Adjust PID settings with the following keys: P and I and D");
pc().printf("\n\r Adjust zero offset with O (oh)");
pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
- pc().printf("HEADING P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset());
+ pc().printf("HEADING P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset());
// handle the key presses
while(1) {
@@ -1733,7 +1743,7 @@
// save pitch PID values for outer loop (must save neutral position also)
configFileIO().saveHeadingData(heading_KP, heading_KI, heading_KD, heading_offset_deg); //_neutral_heading_pos_deg);
- pc().printf("HEADING P: %3.2f, I: %3.2f, D %3.2f, offset: %3.1f mm \r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset());
+ pc().printf("HEADING P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", headingLoop().getControllerP(), headingLoop().getControllerI(), headingLoop().getControllerD(), headingLoop().getOutputOffset());
}
else if (HEADING_PID_key == 'X') {
break; //exit the while loop
@@ -1864,72 +1874,6 @@
}
}
}
-
-//void StateMachine::keyboard_menu_MANUAL_TUNING() {
-// char TUNING_key;
-//
-// // show the menu
-// pc().printf("\r\n1: MANUAL TUNING MENU (EXIT WITH 'X' !)");
-// pc().printf("\r\n(Adjust BCE and BATT positions in real-time. Timeout NOT running! (decrease/increase BCE with A/S, BATT with Q/W)\r\n");
-// pc().printf("MANUAL_TUNING: BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
-//
-// // what needs to be started?
-// bce().setPosition_mm(160);
-//
-// batt().setPosition_mm(50);
-//
-// bce().unpause(); //this is now active
-// batt().unpause(); //this is now active
-//
-// while (1) {
-// if (pc().readable()) {
-// TUNING_key = pc().getc(); //get each keystroke
-// }
-//
-// else {
-// pc().printf("MT: ACTUAL POS (SET POS): BCE: %0.1f (%0.1f), BATT: %0.1f (%0.1f)\r\n",bce().getPosition_mm(),bce().getSetPosition_mm(),batt().getPosition_mm(),batt().getSetPosition_mm());
-// continue; // didn't get a user input, so keep waiting for it
-// }
-//
-// // process the keys
-// if (TUNING_key == 'X') {
-// // STOP THE MOTORS BEFORE LEAVING! (Just in case.)
-// bce().pause();
-// batt().pause();
-//
-// break; //exit the while loop
-// }
-//
-// else if (TUNING_key == 'A') {
-// _neutral_bce_pos_mm = _neutral_bce_pos_mm - 1;
-// bce().setPosition_mm(_neutral_bce_pos_mm); //this variable is loaded from the file at initialization
-// pc().printf("MANUAL_TUNING: (BCE CHANGE: %0.1f) BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
-// }
-//
-// else if (TUNING_key == 'S') {
-// _neutral_bce_pos_mm = _neutral_bce_pos_mm + 1;
-// bce().setPosition_mm(_neutral_bce_pos_mm); //this variable is loaded from the file at initialization
-// pc().printf("MANUAL_TUNING: (BCE CHANGE: %0.1f) BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",bce().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
-// }
-//
-// else if (TUNING_key == 'Q') {
-// _neutral_batt_pos_mm = _neutral_batt_pos_mm - 1;
-// batt().setPosition_mm(_neutral_batt_pos_mm); //this variable is loaded from the file at initialization
-// pc().printf("MANUAL_TUNING: (BATT CHANGE: %0.1f) BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
-// }
-//
-// else if (TUNING_key == 'W') {
-// _neutral_batt_pos_mm = _neutral_batt_pos_mm + 1;
-// batt().setPosition_mm(_neutral_batt_pos_mm); //this variable is loaded from the file at initialization
-// pc().printf("MANUAL_TUNING: (BATT CHANGE: %0.1f) BCE_position: %0.1f, BATT_position: %0.1f (depth: %0.1f ft, pitch: %0.1f deg)\r",batt().getSetPosition_mm(),bce().getPosition_mm(),batt().getPosition_mm(),depthLoop().getPosition(),pitchLoop().getPosition());
-// }
-//
-// else {
-// pc().printf("\r\nThis key does nothing here. ");
-// }
-//
-// }
-//}
void StateMachine::keyboard_menu_CHANNEL_READINGS() {
char TUNING_key;
@@ -2013,16 +1957,71 @@
void StateMachine::keyboard_menu_BCE_PID_settings() {
char PID_key;
- float gain_step_size = 0.01; // modify this to change gain step size
- float BCE_KP = bce().getControllerP(); // load current value
- float BCE_KI = bce().getControllerI(); // load current global value
- float BCE_KD = bce().getControllerD(); // load current global value
+
+ float bce_KP = bce().getControllerP(); // load current value
+ float bce_KI = bce().getControllerI(); // load current global value
+ float bce_KD = bce().getControllerD(); // load current global value
// show the menu
- pc().printf("\r\n1: Buoyancy Engine PID gain settings (MENU)");
- pc().printf("\r\n(Adjust PID settings with the following keys: -= and [] and ;'");
- pc().printf("\r\n(Hit shift + X to exit w/o saving. Hit shift + S to save.)\r\n\n\n");
- pc().printf("bce P: %3.2f, I: %3.2f, D %3.2f, zero %d, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
+ pc().printf("\n\rBuoyancy Engine PID gain settings (MENU)");
+ pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
+ 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.3f, I: %3.3f, D %3.3f, zero %d, 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 == 'S') { // user wants to save these modified values
+ // set values
+ bce().setControllerP(bce_KP);
+ bce().setControllerI(bce_KI);
+ bce().setControllerD(bce_KD);
+
+ // save to "BATT.TXT" file
+ configFileIO().saveBCEData(bce_KP, bce_KI, bce_KD);
+ pc().printf("bce P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope());
+ }
+ else if (PID_key == 'X') {
+ break; //exit the while loop
+ }
+ else if (PID_key == 'P') {
+ pc().printf(">> Type in proportional gain with keyboard.\n\r");
+ bce_KP = getFloatUserInput();
+ }
+ else if (PID_key == 'I') {
+ pc().printf(">> Type in integral gain with keyboard.\n\r");
+ bce_KI = getFloatUserInput();
+ }
+ else if (PID_key == 'D') {
+ pc().printf(">> Type in derivative gain with keyboard.\n\r");
+ bce_KD = getFloatUserInput();
+ }
+ else {
+ pc().printf("\n\rBCE: [%c] This key does nothing here. \r", PID_key);
+ }
+ }
+}
+
+void StateMachine::keyboard_menu_BATT_PID_settings() {
+ char PID_key;
+
+ float batt_KP = batt().getControllerP(); // load current global value
+ float batt_KI = batt().getControllerI(); // load current global value
+ float batt_KD = batt().getControllerD(); // load current global value
+
+ // print the menu
+ pc().printf("\n\rBattery Motor PID gain settings (MENU)");
+ pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
+ pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
+ pc().printf("batt P: %3.3f, I: %3.3f, D %3.3f, zero %d, 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) {
@@ -2035,127 +2034,49 @@
}
// handle the user's key input
- if (PID_key == '-') {
- BCE_KP -= gain_step_size;
- pc().printf("P gain: %0.5f \r\n", BCE_KP);
- }
- else if (PID_key == '=') {
- BCE_KP += gain_step_size;
- pc().printf("P gain: %0.5f \r\n", BCE_KP);
- }
- else if (PID_key == '[') {
- BCE_KI -= gain_step_size;
- pc().printf("I gain: %0.5f \r\n", BCE_KI);
- }
- else if (PID_key == ']') {
- BCE_KI += gain_step_size;
- pc().printf("I gain: %0.5f \r\n", BCE_KI);
- }
- else if (PID_key == ';') {
- BCE_KD -= gain_step_size;
- pc().printf("D gain: %0.5f \r\n", BCE_KD);
- }
- else if (PID_key == '\'') {
- BCE_KD += gain_step_size;
- pc().printf("D gain: %0.5f \r\n", BCE_KD);
- }
- else if (PID_key == 'S') { // user wants to save these modified values
+ if (PID_key == 'S') { // user wants to save these modified values
// set values
- bce().setControllerP(BCE_KP);
- bce().setControllerI(BCE_KI);
- bce().setControllerD(BCE_KD);
+ batt().setControllerP(batt_KP);
+ batt().setControllerI(batt_KI);
+ batt().setControllerD(batt_KD);
// save to "BATT.TXT" file
- configFileIO().saveBCEData(BCE_KP, BCE_KI, BCE_KD);
-
- break; //exit the while loop
+ configFileIO().saveBattData(batt_KP, batt_KI, batt_KD);
+ pc().printf("batt P: %3.3f, I: %3.3f, D %3.3f, zero %d, limit %3.0f mm, slope %3.3f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope());
}
else if (PID_key == 'X') {
break; //exit the while loop
}
- else {
- pc().printf("\r\nThis key does nothing here. ");
+ else if (PID_key == 'P') {
+ pc().printf(">> Type in proportional gain with keyboard.\n\r");
+ batt_KP = getFloatUserInput();
}
- }
-}
-
-void StateMachine::keyboard_menu_BATT_PID_settings() {
- char PID_key;
- float gain_step_size = 0.01; // modify this to change gain step size
- float batt_KP = batt().getControllerP(); // load current global value
- float batt_KI = batt().getControllerI(); // load current global value
- float batt_KD = batt().getControllerD(); // load current global value
-
- // print the menu
- pc().printf("\r\n2: Battery Motor PID gain settings (MENU)");
- pc().printf("\r\n(Adjust PID settings with the following keys: -= and [] and ;'");
- pc().printf("\r\n(Hit shift + X to exit w/o saving. Hit shift + S to save.\r\n");
- pc().printf("batt P: %3.2f, I: %3.2f, D %3.2f, zero %d, 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 if (PID_key == 'I') {
+ pc().printf(">> Type in integral gain with keyboard.\n\r");
+ batt_KI = getFloatUserInput();
+ }
+ else if (PID_key == 'D') {
+ pc().printf(">> Type in derivative gain with keyboard.\n\r");
+ batt_KD = getFloatUserInput();
}
else {
- continue; // didn't get a user input, so keep waiting for it
- }
-
- // handle the user's key input
- if (PID_key == '-') {
- batt_KP -= gain_step_size;
- pc().printf("\rP gain: %0.5f ", batt_KP);
- }
- else if (PID_key == '=') {
- batt_KP += gain_step_size;
- pc().printf("\rP gain: %0.5f ", batt_KP);
- }
- else if (PID_key == '[') {
- batt_KI -= gain_step_size;
- pc().printf("\rI gain: %0.5f ", batt_KI);
- }
- else if (PID_key == ']') {
- batt_KI += gain_step_size;
- pc().printf("\rI gain: %0.5f ", batt_KI);
- }
- else if (PID_key == ';') {
- batt_KD -= gain_step_size;
- pc().printf("\rD gain: %0.5f ", batt_KD);
- }
- else if (PID_key == '\'') {
- batt_KD += gain_step_size;
- pc().printf("\rD gain: %0.5f ", batt_KD);
- }
- else if (PID_key == 'S') { // user wants to save the modified values
- // set global values
- batt().setControllerP(batt_KP);
- batt().setControllerI(batt_KI);
- batt().setControllerD(batt_KD);
-
- // save to "BATT.TXT" file
- configFileIO().saveBattData(batt_KP, batt_KI, batt_KD);
-
- break; //exit the while loop
- }
- else if (PID_key == 'X') {
- break; //exit the while loop
- }
- else {
- pc().printf("This key does nothing here.\r");
+ pc().printf("\n\rBATT: [%c] This key does nothing here. \r", PID_key);
}
}
}
void StateMachine::keyboard_menu_DEPTH_PID_settings() {
char PID_key;
- float gain_step_size = 0.01; // modify this to change gain step size
+
+ float depth_KP = depthLoop().getControllerP(); // load current depth value
+ float depth_KI = depthLoop().getControllerI(); // load current depth value
+ float depth_KD = depthLoop().getControllerD(); // load current depth value
- // show the menu
- pc().printf("\r\n3: DEPTH outer loop PID gain settings (MENU)");
- pc().printf("\r\n(Adjust PID settings with the following keys: -= and [] and ;'");
- pc().printf("\r\n(Hit shift + X to exit w/o saving. Hit shift + S to save.\r\n\n\n");
- 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());
+ // print the menu
+ pc().printf("\n\rDEPTH (Buoyancy Engine O.L.) PID gain settings (MENU)");
+ pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
+ 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.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
// handle the key presses
while(1) {
@@ -2168,59 +2089,55 @@
}
// handle the user's key input
- if (PID_key == '-') {
- _depth_KP -= gain_step_size;
- pc().printf("P gain: %0.5f \r\n", _depth_KP);
- }
- else if (PID_key == '=') {
- _depth_KP += gain_step_size;
- pc().printf("P gain: %0.5f \r\n", _depth_KP);
- }
- else if (PID_key == '[') {
- _depth_KI -= gain_step_size;
- pc().printf("I gain: %0.5f \r\n", _depth_KI);
- }
- else if (PID_key == ']') {
- _depth_KI += gain_step_size;
- pc().printf("I gain: %0.5f \r\n", _depth_KI);
- }
- else if (PID_key == ';') {
- _depth_KD -= gain_step_size;
- pc().printf("D gain: %0.5f \r\n", _depth_KD);
- }
- else if (PID_key == '\'') {
- _depth_KD += gain_step_size;
- pc().printf("D gain: %0.5f \r\n", _depth_KD);
- }
- else if (PID_key == 'S') { // user wants to save these settings
- // set global values
- depthLoop().setControllerP(_depth_KP);
- depthLoop().setControllerI(_depth_KI);
- depthLoop().setControllerD(_depth_KD);
+ if (PID_key == 'S') { // user wants to save these modified values
+ // set values
+ depthLoop().setControllerP(depth_KP);
+ depthLoop().setControllerI(depth_KI);
+ depthLoop().setControllerD(depth_KD);
+
+ // save to "DEPTH.TXT" file
+ configFileIO().saveDepthData(depth_KP, depth_KI, depth_KD, _neutral_bce_pos_mm);
+ pc().printf("DEPTH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset());
- // save depth PID values for outer loop
- configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
- break; //exit the while loop
+ //set class variables that will be used in find neutral sequence
+ _depth_KP = depthLoop().getControllerP(); // load current depth value
+ _depth_KI = depthLoop().getControllerI(); // load current depth value
+ _depth_KD = depthLoop().getControllerD(); // load current depth value
}
else if (PID_key == 'X') {
break; //exit the while loop
}
+ else if (PID_key == 'P') {
+ pc().printf(">> Type in proportional gain with keyboard.\n\r");
+ depth_KP = getFloatUserInput();
+ }
+ else if (PID_key == 'I') {
+ pc().printf(">> Type in integral gain with keyboard.\n\r");
+ depth_KI = getFloatUserInput();
+ }
+ else if (PID_key == 'D') {
+ pc().printf(">> Type in derivative gain with keyboard.\n\r");
+ depth_KD = getFloatUserInput();
+ }
else {
- pc().printf("\r\nThis key does nothing here. ");
+ pc().printf("\n\rDEPTH: [%c] This key does nothing here. \r", PID_key);
}
}
}
void StateMachine::keyboard_menu_PITCH_PID_settings() {
char PID_key;
- float gain_step_size = 0.01; // modify this to change gain step size
+
+ float pitch_KP = pitchLoop().getControllerP(); // load current pitch value
+ float pitch_KI = pitchLoop().getControllerI(); // load current pitch value
+ float pitch_KD = pitchLoop().getControllerD(); // load current pitch value
// print the menu
- pc().printf("\r\n4: Pitch outer loop PID gain settings (MENU)");
- pc().printf("\r\n(Adjust PID settings with the following keys: -= and [] and ;'");
- pc().printf("\r\n(Hit shift + X to exit w/o saving. Hit shift + S to save.\r\n");
- 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());
-
+ pc().printf("\n\rPITCH (Battery Motor O.L.) PID gain settings (MENU)");
+ pc().printf("\n\r(Adjust PID settings with the following keys: P and I and D");
+ pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
+ pc().printf("PITCH P: %3.3f, I: %3.3f, D %3.3f, 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
@@ -2230,47 +2147,39 @@
else {
continue; // didn't get a user input, so keep waiting for it
}
-
+
// handle the user's key input
- if (PID_key == '-') {
- _pitch_KP -= gain_step_size;
- pc().printf("\rP gain: %0.5f ", _pitch_KP);
- }
- else if (PID_key == '=') {
- _pitch_KP += gain_step_size;
- pc().printf("\rP gain: %0.5f ", _pitch_KP);
- }
- else if (PID_key == '[') {
- _pitch_KI -= gain_step_size;
- pc().printf("\rI gain: %0.5f ", _pitch_KI);
- }
- else if (PID_key == ']') {
- _pitch_KI += gain_step_size;
- pc().printf("\rI gain: %0.5f ", _pitch_KI);
- }
- else if (PID_key == ';') {
- _pitch_KD -= gain_step_size;
- pc().printf("\rD gain: %0.5f ", _pitch_KD);
- }
- else if (PID_key == '\'') {
- _pitch_KD += gain_step_size;
- pc().printf("\rD gain: %0.5f ", _pitch_KD);
- }
- else if (PID_key == 'S') { // user wants to save the modified values
- // set global values
- pitchLoop().setControllerP(_pitch_KP);
- pitchLoop().setControllerI(_pitch_KI);
- pitchLoop().setControllerD(_pitch_KD);
-
- // save pitch PID values for outer loop (must save neutral position also)
- configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
- break; //exit the while loop
+ if (PID_key == 'S') { // user wants to save these modified values
+ // set values
+ pitchLoop().setControllerP(pitch_KP);
+ pitchLoop().setControllerI(pitch_KI);
+ pitchLoop().setControllerD(pitch_KD);
+
+ // save to "PITCH.TXT" file
+ configFileIO().savePitchData(pitch_KP, pitch_KI, pitch_KD, _neutral_batt_pos_mm);
+ pc().printf("PITCH P: %3.3f, I: %3.3f, D %3.3f, offset: %3.1f mm \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset());
+
+ _pitch_KP = pitchLoop().getControllerP(); // load current pitch value
+ _pitch_KI = pitchLoop().getControllerI(); // load current pitch value
+ _pitch_KD = pitchLoop().getControllerD(); // load current pitch value
}
else if (PID_key == 'X') {
break; //exit the while loop
}
+ else if (PID_key == 'P') {
+ pc().printf(">> Type in proportional gain with keyboard.\n\r");
+ pitch_KP = getFloatUserInput();
+ }
+ else if (PID_key == 'I') {
+ pc().printf(">> Type in integral gain with keyboard.\n\r");
+ pitch_KI = getFloatUserInput();
+ }
+ else if (PID_key == 'D') {
+ pc().printf(">> Type in derivative gain with keyboard.\n\r");
+ pitch_KD = getFloatUserInput();
+ }
else {
- pc().printf("This key does nothing here.\r");
+ pc().printf("\n\rPITCH: [%c] This key does nothing here. \r", PID_key);
}
}
}
@@ -2478,7 +2387,7 @@
if (valid_input) {
float_conversion = atof(user_string);
- pc().printf("VALID INPUT! Your input was: %f\r\n", float_conversion);
+ pc().printf("VALID INPUT! Your input was: %3.3f (PRESS \"S\" (shift + S) to save!)\r\n", float_conversion);
break;
}
}