Flying Sea Glider
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:
- 21:38c8544db6f4
- Parent:
- 20:8987a9ae2bc7
- Child:
- 22:a10ee088403b
--- a/StateMachine/StateMachine.cpp Wed Nov 22 14:32:06 2017 +0000
+++ b/StateMachine/StateMachine.cpp Wed Nov 22 23:04:11 2017 +0000
@@ -16,29 +16,31 @@
_neutral_sink_timer = 0;
_neutral_rise_timer = 0;
- previousPosition_mm = 220; // Troy: centered, overwritten by the state machine (LOAD THIS FROM CONFIG?)
- // Dan: no, think - where do you Want the bce to start from? Full extend limt probably.
+ previousPosition_mm = 300; // Default is fully extended, loads in find_neutral
_state = SIT_IDLE; // select starting state here
isTimeoutRunning = false; // default timer to not running
isSubStateTimerRunning = false; // default timer to not running
- _neutral_buoyancy_bce_pos_mm = 0;
- _neutral_buoyancy_batt_pos_mm = 0;
+ _state_counter = 0;
+
+ _neutral_sub_state_active = false;
- //new
- _next_state = -1; //next state is used to prevent states from changing as the FSM executes
- _state_counter = 0;
+ _depth_KP = depthLoop().getControllerP(); // load current depth value
+ _depth_KI = depthLoop().getControllerI(); // load current depth value
+ _depth_KD = depthLoop().getControllerD(); // load current depth value
+
+ _pitch_KP = pitchLoop().getControllerP(); // load current pitch value
+ _pitch_KI = pitchLoop().getControllerI(); // load current pitch value
+ _pitch_KD = pitchLoop().getControllerD(); // load current pitch value
+
+ _neutral_bce_pos_mm = depthLoop().getOutputOffset(); //load current neutral buoyancy position offset
+ _neutral_batt_pos_mm = pitchLoop().getOutputOffset(); //load current neutral buoyancy position offset
}
//Finite State Machine (FSM)
void StateMachine::runStateMachine() {
-
- static bool runFirstNeutral = false;
-
- //use the _next_state when the state machine is run again (so that it cannot change states while the FSM executes)
- if (_next_state > -1)
- _state = _next_state; //current state comes from the state at the completion of the FSM
+ //static bool runFirstNeutral = false;
// finite state machine ... each state has at least one exit criteria
switch (_state) {
@@ -81,13 +83,13 @@
// how exit?
if (timer > _timeout) {
pc().printf("EC: timed out\r\n");
- _next_state = FLOAT_BROADCAST;
+ _state = FLOAT_BROADCAST;
timer.reset();
isTimeoutRunning = false;
}
else if (depthLoop().getPosition() < 0.2) { //if the depth is greater than 0.2 feet, go to float broadcast
pc().printf("EC: depth: %3.1f, cmd: 0.5 [%0.1f sec]\r",depthLoop().getPosition(), timer.read());
- _next_state = FLOAT_BROADCAST;
+ _state = FLOAT_BROADCAST;
timer.reset();
isTimeoutRunning = false;
}
@@ -96,7 +98,7 @@
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");
+ pc().printf("\r\n\nstate: FIND_NEUTRAL\n\r");
timer.reset(); // timer goes back to zero
timer.start(); // background timer starts running
isTimeoutRunning = true;
@@ -105,35 +107,34 @@
bce().unpause();
batt().unpause();
- //first iteration sends SINKING sub-state
- runFirstNeutral = true;
+ //load the current position of the BCE piston into the sub-FSM
+ previousPosition_mm = bce().getPosition_mm();
+ pc().printf("FN: Neutral sequence, BCE piston at %3.1f\n\r", previousPosition_mm);
+
+ //first iteration goes into Neutral Finding Sub-FSM
+ //_neutral_sub_state_active = true;
+ //_sub_state = runNeutralStateMachine(); //send first state of sub-FSM
+
+ runNeutralStateMachine(); //send first state of sub-FSM
}
// how exit? (exit with the timer, if timer still running continue processing sub FSM)
if (timer > _timeout) {
pc().printf("FN: timed out [time: %0.1f sec]\r\n", timer.read());
- _next_state = EMERGENCY_CLIMB; //new behavior (if this times out it emergency surfaces)
+ _state = EMERGENCY_CLIMB; //new behavior (if this times out it emergency surfaces)
timer.reset();
isTimeoutRunning = false;
}
-
- // what is active? (call neutral finding sub-function every iteration)
- if (runFirstNeutral) {
- //pc().printf("First iteration of find neutral\n\r"); // debug (confirmed working)
- _sub_state = findNeutralSubState(NEUTRAL_SINKING);
- _previous_sub_state = _sub_state; //save previous sub-state
- runFirstNeutral = false;
+
+ //what is active? (neutral finding sub-function runs until completion)
+ if (runNeutralStateMachine() == NEUTRAL_EXIT) {
+ //if the current state is NEUTRAL_EXIT stop running the sub FSM
+
+ //if successful, FIND_NEUTRAL then goes to RISE
+ pc().printf("*************************************** FIND_NEUTRAL sequence complete. Rising.\n\n\r");
+ _state = RISE;
}
- else { //second and further iterations, this is running
- _sub_state = findNeutralSubState(_previous_sub_state);
- _previous_sub_state = _sub_state; //save previous sub-state
-
- //if sub-FSM has completed its state machine
- if (_sub_state == NEUTRAL_EXIT) {
- pc().printf("DEBUG: FIND_NEUTRAL sub FSM completed!\n\r");
- _next_state = RISE; //your next state will be RISE in the FSM
- }
- }
+
break;
case DIVE :
@@ -158,19 +159,19 @@
// how exit?
if (timer > _timeout) {
pc().printf("DIVE: timed out\n\n\r");
- _next_state = RISE; //new behavior 11/17/2017
+ _state = RISE; //new behavior 11/17/2017
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());
- _next_state = RISE;
+ _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.2f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), timer.read());
+ pc().printf("DIVE: bce pos: %3.1f mm, batt pos: %3.1f mm (depth: %3.1f ft) (pitch: %3.1f deg)[%0.2f sec]\r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition(), pitchLoop().getPosition(), timer.read());
bce().setPosition_mm(depthLoop().getOutput());
batt().setPosition_mm(pitchLoop().getOutput());
break;
@@ -197,13 +198,13 @@
// how exit?
if (timer > _timeout) {
pc().printf("RISE: timed out\r\n");
- _next_state = EMERGENCY_CLIMB;
+ _state = EMERGENCY_CLIMB;
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());
- _next_state = FLOAT_LEVEL;
+ _state = FLOAT_LEVEL;
timer.reset();
isTimeoutRunning = false;
}
@@ -234,13 +235,13 @@
// how exit?
if (timer > _timeout) {
pc().printf("FL: timed out\r\n");
- _next_state = FLOAT_BROADCAST;
+ _state = FLOAT_BROADCAST;
timer.reset();
isTimeoutRunning = false;
}
else if (fabs(imu().getPitch() - pitchLoop().getCommand()) < fabs(pitchTolerance)) {
pc().printf("FL: pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), pitchTolerance);
- _next_state = FLOAT_BROADCAST;
+ _state = FLOAT_BROADCAST;
timer.reset();
isTimeoutRunning = false;
}
@@ -270,14 +271,14 @@
// how exit?
if (timer > _timeout) {
pc().printf("FB: timed out\r\n");
- _next_state = SIT_IDLE;
+ _state = SIT_IDLE;
timer.reset();
isTimeoutRunning = false;
}
else if ( (fabs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) and
(fabs(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());
- _next_state = SIT_IDLE;
+ _state = SIT_IDLE;
timer.reset();
isTimeoutRunning = false;
}
@@ -297,34 +298,27 @@
// what needs to be started?
bce().unpause();
batt().unpause();
-
- //NEW: retrieve depth and pitch commands from config file struct
- // concept is to load this each time the multi-dive restarts
- //keyboard starts sequence at 0 from keyboard function
- //stateMachine().getDiveSequence();
-
- //retrieve commands from structs
+ //retrieve commands from structs (loaded from sequence.cfg file)
float sequenceDepthCommand = currentStateStruct.depth;
float sequencePitchCommand = currentStateStruct.pitch;
// what are the commands?
depthLoop().setCommand(sequenceDepthCommand);
pitchLoop().setCommand(sequencePitchCommand);
- pc().printf("MULTI-DIVE: depth cmd: %3.1f\r\n",depthLoop().getCommand());
- pc().printf("MULTI-DIVE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
+ pc().printf("MULTI-DIVE: depth cmd: %3.1f ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
}
// how exit?
if (timer > _timeout) {
- pc().printf("MULTI-DIVE: timed out\n\n\r");
- _next_state = MULTI_RISE; //new behavior 11/17/2017
+ pc().printf("\n\n\rMULTI-DIVE: timed out [time: %0.1f]\n\n\r", timer.read());
+ _state = MULTI_RISE; //new behavior 11/17/2017
timer.reset();
isTimeoutRunning = false;
}
else if (depthLoop().getPosition() > depthLoop().getCommand()) {
pc().printf("MULTI-DIVE: depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
- _next_state = MULTI_RISE;
+ _state = MULTI_RISE;
timer.reset();
isTimeoutRunning = false;
}
@@ -356,15 +350,14 @@
// what are the commands? (send back to 0.5 feet, not surface) // 11/21/2017
depthLoop().setCommand(0.5);
- pitchLoop().setCommand(-sequencePitchCommand);
- pc().printf("MULTI-RISE: depth cmd: 0.0\r\n");
- pc().printf("MULTI-RISE: pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
+ pitchLoop().setCommand(-sequencePitchCommand);
+ pc().printf("MULTI-RISE: depth cmd: 0.0 ft, pitch cmd: %3.1f deg\r\n",depthLoop().getCommand(), pitchLoop().getCommand());
}
// how exit?
if (timer > _timeout) {
- pc().printf("MULTI-RISE: timed out\r\n");
- _next_state = EMERGENCY_CLIMB;
+ pc().printf("MULTI-RISE: timed out [time: %0.1f]\n\n\r", timer.read());
+ _state = EMERGENCY_CLIMB;
timer.reset();
isTimeoutRunning = false;
@@ -386,12 +379,12 @@
//check if this is the end of the dive sequence
//CHECK BEFORE ANYTHING ELSE that you have reached the "exit" state (Float_Level)
if (currentStateStruct.state == FLOAT_LEVEL) {
- _next_state = FLOAT_LEVEL;
+ _state = FLOAT_LEVEL;
return;
}
else
- _next_state = MULTI_DIVE;
+ _state = MULTI_DIVE;
//have to stop this with the _state_counter variable!
}
@@ -404,7 +397,7 @@
default :
pc().printf("DEBUG: SIT_IDLE\n\r");
- _next_state = SIT_IDLE;
+ _state = SIT_IDLE;
}
}
@@ -436,19 +429,18 @@
//Find Neutral sub finite state machine
// Note: the sub-fsm only moves the pistons once at the start of each timer loop
// (timer completes, move piston, timer completes, move piston, etc)
-int StateMachine::findNeutralSubState(int input_substate) {
-
- //output substate is the input substate unless it is changed in the sub FSM
- static int output_substate = NEUTRAL_SINKING; //to start
+int StateMachine::runNeutralStateMachine() {
+ static int substate = NEUTRAL_SINKING; //to start
- switch (input_substate) {
+ pc().printf("Neutral FSM: %d [time: %0.1f] (depth: %0.1f, pitch: %0.1f)\r", substate, timer.read(), depthLoop().getPosition(), pitchLoop().getPosition());
+
+ switch (substate) {
case NEUTRAL_SINKING :
//start the 10 second timer
if (!isSubStateTimerRunning) {
- pc().printf(" input_substate: %d\n\r", input_substate); //debug
_neutral_sink_timer = timer.read() + 10; //record the time when this block is first entered and add 10 seconds
- pc().printf("\r\n\n START NEUTRAL_SINKING (_neutral_sink_timer will go off at %0.1f sec) %3.1f\r\n", _neutral_sink_timer, timer.read());
+ pc().printf("\r\n\nSTART NEUTRAL_SINKING (next retraction at %0.1f sec) [current time: %0.1f]\n\r", _neutral_sink_timer, timer.read());
// what are the commands?
//move piston at start of sequence (retract 10 mm)
@@ -462,29 +454,32 @@
// how exit?
//once reached the travel limit, no need to keep trying, so exit
if (bce().getPosition_mm() <= 0) {
- output_substate = NEUTRAL_EXIT;
+ substate = NEUTRAL_EXIT;
}
//once deeper than the commanded setpoint...
else if (depthLoop().getPosition() > depthCommand) {
- output_substate = NEUTRAL_SLOWLY_RISE; // next state
+ substate = NEUTRAL_SLOWLY_RISE; // next state
isSubStateTimerRunning = false; //reset the sub state timer
}
// what is active?
//once the 10 second timer is complete, reset the timeout so the state one-shot entry will move the setpoint
if (timer.read() >= _neutral_sink_timer) {
- pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE! %3.1f\r\n", timer.read());
- pc().printf("\r\n\n (BATT POS: %0.1f) retract 10 mm (time: %3.1f)\r\n", previousPosition_mm, timer.read());
+ pc().printf("\r\n\n NEUTRAL_SINKING TIMER COMPLETE!");
+ pc().printf("\r\n\n (BATT POS: %0.1f) retract 10 mm [current time: %3.1f]\r\n", previousPosition_mm, timer.read());
isSubStateTimerRunning = false; //reset the sub state timer
}
+
+
break;
case NEUTRAL_SLOWLY_RISE:
if (!isSubStateTimerRunning) {
- pc().printf("\r\n\n(9) START NEUTRAL_SLOWLY_RISE (will go off at %0.1f sec)\r\n", timer.read());
_neutral_rise_timer = timer.read()+ 5; //record the time when this block is first entered and add 5 seconds
+ pc().printf("\r\n\nSTART NEUTRAL_SLOWLY_RISE (next retraction at %0.1f sec) [current time: %0.1f]\r\n",_neutral_rise_timer,timer.read());
+
isSubStateTimerRunning = true; //disable this block after one iteration
// what are the commands?
@@ -497,19 +492,19 @@
// how exit?
//once at full travel limit and haven't yet risen, time to give up and exit
if (bce().getPosition_mm() >= bce().getTravelLimit()) {
- output_substate = NEUTRAL_EXIT;
+ substate = NEUTRAL_EXIT;
}
//depth rate or sink rate < 0 ft/s, go to the next substate the next iteration
else if (depthLoop().getVelocity() < 0) { //less than zero ft/s
- pc().printf("\r\n\n(9) NEUTRAL_SLOWLY_RISE: Sink Rate below 0 ft/s [time: %0.1f]\r\n", timer.read());
- output_substate = NEUTRAL_CHECK_PITCH;
+ pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE: Sink Rate below 0 ft/s [time: %0.1f]\r\n", timer.read());
+ substate = NEUTRAL_CHECK_PITCH;
}
// what is active?
//once 5 second timer complete, reset the timeout so the state one-shot entry will move the setpoint
if (timer.read() >= _neutral_sink_timer) {
- pc().printf("\r\n\n(9)NEUTRAL_SLOWLY_RISE TIMER COMPLETE! (time: %3.1f)\r\n", timer.read());
- pc().printf("\r\n\n (BATT POS: %0.1f) extend 1 mm %3.1f\r\n", previousPosition_mm, timer.read());
+ pc().printf("\r\n\nNEUTRAL_SLOWLY_RISE TIMER COMPLETE!");
+ pc().printf("\r\n\n (BATT POS: %0.1f) extend 1 mm [timer: %0.1f]\r\n", previousPosition_mm, timer.read());
isSubStateTimerRunning = false; // reset the sub state timer
}
@@ -526,33 +521,48 @@
// how exit?
//pitch angle and pitch rate within small tolerance
//benchtop tests confirm angle needs to be around 2 degrees
- if ((fabs(pitchLoop().getPosition()) < 0.5) and (fabs(pitchLoop().getVelocity()) < 2.0)) {
- pc().printf("Saving Positions: BCE: %0.1f mm, BATT: %0.1f\n\r",bce().getPosition_mm(),batt().getPosition_mm());
-
+ if ((fabs(pitchLoop().getPosition()) < 0.5) and (fabs(pitchLoop().getVelocity()) < 2.0)) {
//save positions locally
- depthLoop().setOutputOffset(bce().getPosition_mm());
- pitchLoop().setOutputOffset(batt().getPosition_mm());
+ _neutral_batt_pos_mm = batt().getPosition_mm();
+ _neutral_bce_pos_mm = bce().getPosition_mm();
+
+ //set the neutral positions in each outer loop
+ depthLoop().setOutputOffset(_neutral_bce_pos_mm);
+ pitchLoop().setOutputOffset(_neutral_batt_pos_mm);
- //save to neutral.cfg
- // Troy: save into the depth.txt and pitch.txt files, not a separate file.
- ConfigFileIO().saveNeutralPositions(bce().getPosition_mm(),batt().getPosition_mm()); //BCE, BATT
+ // save into the depth.txt and pitch.txt files
+ configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm); //P,I,D,batt zeroOffset
+ configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm); //P,I,D, bce zeroOffset
- output_substate = NEUTRAL_EXIT; //the "case" NEUTRAL_EXIT is used to tell the greater FSM that this sub-FSM has completed
+ pc().printf("\n\rSaving Positions: BCE: %0.1f mm, BATT: %0.1f\n\n\r",_neutral_bce_pos_mm,_neutral_batt_pos_mm);
+
+ substate = NEUTRAL_EXIT; //the "case" NEUTRAL_EXIT is used to tell the greater FSM that this sub-FSM has completed
}
break;
- case NEUTRAL_EXIT :
- //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed
- pc().printf("substate: NEUTRAL_EXIT\n\r");
+ //this state could be removed, it is only used as a transition but is needed to stop entering this function
+ case NEUTRAL_EXIT :
+ //NEUTRAL_EXIT state is used to tell the greater FSM that this sub-FSM has completed
+ pc().printf("substate: NEUTRAL_EXIT\n\r");
break;
-
- default :
- pc().printf("how did we get to substate default?\n\r"); //debug
- output_substate = NEUTRAL_EXIT; //a default within the sub-state machine
- break;
+
+ default :
+ pc().printf("how did we get to substate default?\n\r"); //debug
+ substate = NEUTRAL_EXIT; //a default within the sub-state machine
+ break;
}
- return output_substate;
+ //check if the sub-FSM has completed
+ if (substate == NEUTRAL_EXIT) {
+ //reset substate to NEUTRAL_SINKING (first state)
+ substate = NEUTRAL_EXIT;
+ //indicate to outer FSM that sub-FSM has completed
+
+ pc().printf("********************************DID YOU EXIT? *******************************\n\n\r");
+ return NEUTRAL_EXIT;
+ }
+ else
+ return substate; //otherwise continue running through sequence
}
// keyboard runs independently of the state machine, handling one key at a time
@@ -563,15 +573,16 @@
// check keyboard and make settings changes as requested
// states can be changed only at the start of a sequence (when the system is in SIT_IDLE)
- if (pc().readable() && (_state == SIT_IDLE)) {
+
+ int _keyboard_state = -1; //made this a local variable because it was retaining the last keyboard state
+
+ if (pc().readable() && _state == SIT_IDLE) {
// get the key
userInput = pc().getc();
- // keyboard resets timer each time it's used
+ // keyboard has to reset timer each time it's used
isTimeoutRunning = false;
- pc().printf("KEYBOARD isTimeoutRunning: %d\n\r", isTimeoutRunning);
-
// check command against desired control buttons
if (userInput == 'D' or userInput == 'd') {
_keyboard_state = DIVE;
@@ -693,12 +704,9 @@
pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
}
- else {
- _keyboard_state = -1;
- }
//when you read the keyboard successfully, change the state
- _next_state = _keyboard_state; //set state at the end of this function
+ _state = _keyboard_state; //set state at the end of this function
}
}
@@ -714,7 +722,7 @@
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());
+ 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());
// handle the key presses
while(1) {
@@ -781,7 +789,7 @@
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());
+ 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) {
@@ -840,9 +848,6 @@
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)");
@@ -862,37 +867,37 @@
// handle the user's key input
if (PID_key == '-') {
- KP -= gain_step_size;
- pc().printf("P gain: %0.5f \r\n", KP);
+ _depth_KP -= gain_step_size;
+ pc().printf("P gain: %0.5f \r\n", _depth_KP);
}
else if (PID_key == '=') {
- KP += gain_step_size;
- pc().printf("P gain: %0.5f \r\n", KP);
+ _depth_KP += gain_step_size;
+ pc().printf("P gain: %0.5f \r\n", _depth_KP);
}
else if (PID_key == '[') {
- KI -= gain_step_size;
- pc().printf("I gain: %0.5f \r\n", KI);
+ _depth_KI -= gain_step_size;
+ pc().printf("I gain: %0.5f \r\n", _depth_KI);
}
else if (PID_key == ']') {
- KI += gain_step_size;
- pc().printf("I gain: %0.5f \r\n", KI);
+ _depth_KI += gain_step_size;
+ pc().printf("I gain: %0.5f \r\n", _depth_KI);
}
else if (PID_key == ';') {
- KD -= gain_step_size;
- pc().printf("D gain: %0.5f \r\n", KD);
+ _depth_KD -= gain_step_size;
+ pc().printf("D gain: %0.5f \r\n", _depth_KD);
}
else if (PID_key == '\'') {
- KD += gain_step_size;
- pc().printf("D gain: %0.5f \r\n", KD);
+ _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(KP);
- depthLoop().setControllerI(KI);
- depthLoop().setControllerD(KD);
+ depthLoop().setControllerP(_depth_KP);
+ depthLoop().setControllerI(_depth_KI);
+ depthLoop().setControllerD(_depth_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);
+ // save depth PID values for outer loop
+ configFileIO().saveDepthData(_depth_KP, _depth_KI, _depth_KD, _neutral_bce_pos_mm);
break; //exit the while loop
}
else if (PID_key == 'X') {
@@ -907,9 +912,6 @@
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)");
@@ -929,36 +931,37 @@
// handle the user's key input
if (PID_key == '-') {
- KP -= gain_step_size;
- pc().printf("\rP gain: %0.5f ", KP);
+ _pitch_KP -= gain_step_size;
+ pc().printf("\rP gain: %0.5f ", _pitch_KP);
}
else if (PID_key == '=') {
- KP += gain_step_size;
- pc().printf("\rP gain: %0.5f ", KP);
+ _pitch_KP += gain_step_size;
+ pc().printf("\rP gain: %0.5f ", _pitch_KP);
}
else if (PID_key == '[') {
- KI -= gain_step_size;
- pc().printf("\rI gain: %0.5f ", KI);
+ _pitch_KI -= gain_step_size;
+ pc().printf("\rI gain: %0.5f ", _pitch_KI);
}
else if (PID_key == ']') {
- KI += gain_step_size;
- pc().printf("\rI gain: %0.5f ", KI);
+ _pitch_KI += gain_step_size;
+ pc().printf("\rI gain: %0.5f ", _pitch_KI);
}
else if (PID_key == ';') {
- KD -= gain_step_size;
- pc().printf("\rD gain: %0.5f ", KD);
+ _pitch_KD -= gain_step_size;
+ pc().printf("\rD gain: %0.5f ", _pitch_KD);
}
else if (PID_key == '\'') {
- KD += gain_step_size;
- pc().printf("\rD gain: %0.5f ", KD);
+ _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(KP);
- pitchLoop().setControllerI(KI);
- pitchLoop().setControllerD(KD);
+ pitchLoop().setControllerP(_pitch_KP);
+ pitchLoop().setControllerI(_pitch_KI);
+ pitchLoop().setControllerD(_pitch_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);
+ // save pitch PID values for outer loop
+ configFileIO().savePitchData(_pitch_KP, _pitch_KI, _pitch_KD, _neutral_batt_pos_mm);
break; //exit the while loop
}
else if (PID_key == 'X') {
@@ -981,7 +984,7 @@
void StateMachine::setState(int input_state) {
//pc().printf("input_state: %d\n\r", input_state); //debug
//_state = input_state; //changing wrong variable
- _next_state = input_state;
+ _state = input_state;
}
int StateMachine::getState() {
@@ -1001,10 +1004,10 @@
}
void StateMachine::setNeutralPositions(float batt_pos_mm, float bce_pos_mm) {
- _neutral_buoyancy_batt_pos_mm = batt_pos_mm;
- _neutral_buoyancy_bce_pos_mm = bce_pos_mm;
+ _neutral_batt_pos_mm = batt_pos_mm;
+ _neutral_bce_pos_mm = bce_pos_mm;
- pc().printf("Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\n\r",_neutral_buoyancy_batt_pos_mm,_neutral_buoyancy_bce_pos_mm);
+ pc().printf("Neutral Buoyancy Positions: batt: %0.1f, bce: %0.1f\n\r",_neutral_batt_pos_mm,_neutral_bce_pos_mm);
}
int StateMachine::timeoutRunning() {