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:
- 76:c802e1da4179
- Parent:
- 74:d281aaef9766
- Child:
- 80:4e5d306d695b
--- a/StateMachine/StateMachine.cpp Thu Aug 16 13:57:19 2018 +0000 +++ b/StateMachine/StateMachine.cpp Thu Aug 16 21:47:40 2018 +0000 @@ -1131,8 +1131,8 @@ xbee().printf("Max recorded depth: neutral: %0.1f, dive: %0.1f, auto_neutral_depth: %0.1f\r\n\n",_max_recorded_depth_neutral, _max_recorded_depth_dive, _max_recorded_auto_neutral_depth); xbee().printf("\r\n"); - xbee().printf("bce P:%6.2f, I:%6.2f, D:%6.2f, zero offset: %3i, limit %6.1f mm, slope %0.5f, deadband: %0.1f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope(), bce().getDeadband()); - xbee().printf("batt P:%6.2f, I:%6.2f, D:%6.2f, zero offset: %3i, limit %6.1f mm, slope %0.5f, deadband: %0.1f \r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope(), batt().getDeadband()); + xbee().printf("bce P:%6.2f, I:%6.2f, D:%6.2f, zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope(), bce().getFilterFrequency(), bce().getDeadband()); + xbee().printf("batt P:%6.2f, I:%6.2f, D:%6.2f, zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope(), batt().getFilterFrequency(), batt().getDeadband()); xbee().printf("rudder min_pwm:%6.1f, center_pwm:%6.1f, max_pwm:%6.1f (min_deg:%6.1f max_deg:%6.1f)\r\n",rudder().getMinPWM(),rudder().getCenterPWM(),rudder().getMaxPWM(),rudder().getMinDeg(),rudder().getMaxDeg()); xbee().printf("depth P:%6.2f, I:%6.2f, D:%6.2f, output offset: %6.1f mm, filter_freq: %0.1f, deadband: %0.1f \r\n", depthLoop().getControllerP(), depthLoop().getControllerI(), depthLoop().getControllerD(), depthLoop().getOutputOffset(),depthLoop().getFilterFrequency(),depthLoop().getDeadband()); xbee().printf("pitch P:%6.2f, I:%6.2f, D:%6.2f, output offset: %6.1f mm, filter_freq: %0.1f, deadband: %0.1f \r\n", pitchLoop().getControllerP(), pitchLoop().getControllerI(), pitchLoop().getControllerD(), pitchLoop().getOutputOffset(),pitchLoop().getFilterFrequency(),pitchLoop().getDeadband()); @@ -1640,54 +1640,25 @@ } } } - -//void StateMachine::keyboard_menu_POSITION_READINGS() { -// char TUNING_key; -// -// // show the menu -// xbee().printf("\r\n9: BCE and BMM POSITION READINGS (EXIT WITH 'X' !)"); -// -// while (1) { -// if (xbee().readable()) { -// TUNING_key = xbee().getc(); //get each keystroke -// } -// -// // 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 { -// // Testing out ADC -// wait(0.5); -// float vref = 5.6; -// float vmeasured = 0; -// unsigned int raw = adc().readCh5(); -// vmeasured = ((float)raw)/4095.0*vref; -// -// xbee().printf("BCE POS(%d), BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d) << POS: BCE %0.2f, BATT %0.2f >>\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),bce().getPosition_mm(),batt().getPosition_mm()); -// -// continue; // didn't get a user input, so keep waiting for it -// } -// } -//} void StateMachine::keyboard_menu_BCE_PID_settings() { char BCE_PID_key; - 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 + // load current values from files + float bce_KP = bce().getControllerP(); + float bce_KI = bce().getControllerI(); + float bce_KD = bce().getControllerD(); + + float bce_deadband = bce().getDeadband(); + float bce_frequency = bce().getFilterFrequency(); + int bce_zero_offset = bce().getZeroCounts(); + //BCE frequency and deadband are hardcoded! // show the menu - xbee().printf("\n\rBuoyancy Engine PID gain settings (MENU)"); - xbee().printf("\n\rAdjust PID settings with the following keys: P I D. Filter = F, deadband = B.\n\r"); + xbee().printf("\n\rBuoyancy Engine PID gain settings (MENU). ADJUST WITH CARE!"); + xbee().printf("\n\rAdjust PID settings with the following keys: P I D. Filter = F, deadband = B, zero offset = Z\n\r"); xbee().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r"); - xbee().printf("bce P: %3.3f, I: %3.3f, D %3.3f, zeroOffset %d, limit %3.0f mm, slope %3.3f \r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope()); + xbee().printf("bce P:%6.2f, I:%6.2f, D:%6.2f, zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope(), bce().getFilterFrequency(), bce().getDeadband()); // handle the key presses while(1) { @@ -1704,11 +1675,16 @@ // set values bce().setControllerP(bce_KP); bce().setControllerI(bce_KI); - bce().setControllerD(bce_KD); + bce().setControllerD(bce_KD); + + bce().setDeadband(bce_deadband); + bce().setFilterFrequency(bce_frequency); + bce().setZeroCounts(bce_zero_offset); //integer value // save to "BCE.TXT" file - configFileIO().saveBCEData(bce_KP, bce_KI, bce_KD, 100, 6.0, 0.5); - xbee().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()); + //saveBattData(float batt_p_gain, float batt_i_gain, float batt_d_gain, int batt_zeroOffset, float batt_filter_freq, float batt_deadband) + configFileIO().saveBCEData(bce_KP, bce_KI, bce_KD, bce_zero_offset, bce_frequency, bce_deadband); + xbee().printf("bce P:%6.2f, I:%6.2f, D:%6.2f, zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", bce().getControllerP(), bce().getControllerI(), bce().getControllerD(), bce().getZeroCounts(), bce().getTravelLimit(), bce().getPotSlope(), bce().getFilterFrequency(), bce().getDeadband()); } else if (BCE_PID_key == 'X') { break; //exit the while loop @@ -1727,13 +1703,15 @@ } else if (BCE_PID_key == 'F') { xbee().printf(">> Type in FILTER FREQUENCY with keyboard.\n\r"); - float bce_freq = getFloatUserInput(); - bce().setFilterFrequency(bce_freq); + bce_frequency = getFloatUserInput(); } else if (BCE_PID_key == 'B') { xbee().printf(">> Type in DEADBAND with keyboard.\n\r"); - float bce_deadband = getFloatUserInput(); - bce().setDeadband(bce_deadband); + bce_deadband = getFloatUserInput(); + } + else if (BCE_PID_key == 'Z') { + xbee().printf(">> Type in zero count offset with keyboard.\n\r"); + bce_zero_offset = (int)getFloatUserInput(); } else { xbee().printf("\n\rBCE: [%c] This key does nothing here. \r", BCE_PID_key); @@ -1743,17 +1721,23 @@ void StateMachine::keyboard_menu_BATT_PID_settings() { char BMM_PID_key; + + // load current values from files + float batt_KP = batt().getControllerP(); + float batt_KI = batt().getControllerI(); + float batt_KD = batt().getControllerD(); - 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 + float batt_deadband = batt().getDeadband(); + float batt_frequency = batt().getFilterFrequency(); + int batt_zero_offset = batt().getZeroCounts(); + //BATT frequency and deadband are hardcoded! // print the menu xbee().printf("\n\rBattery Motor PID gain settings (MENU)"); xbee().printf("\n\rAdjust PID settings with the following keys: P I D. Filter = F, deadband = B.\n\r"); xbee().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r"); - xbee().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()); - + xbee().printf("batt P:%6.2f, I:%6.2f, D:%6.2f, zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope(), batt().getFilterFrequency(), batt().getDeadband()); + // handle the key presses while(1) { // get the user's keystroke from either of the two inputs @@ -1769,11 +1753,16 @@ // set values batt().setControllerP(batt_KP); batt().setControllerI(batt_KI); - batt().setControllerD(batt_KD); + batt().setControllerD(batt_KD); + + batt().setDeadband(batt_deadband); + batt().setFilterFrequency(batt_frequency); + batt().setZeroCounts(batt_zero_offset); //integer value // save to "BATT.TXT" file - configFileIO().saveBattData(batt_KP, batt_KI, batt_KD, 100, 6.0, 0.5); - xbee().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()); + //saveBCEData(float bce_p_gain, float bce_i_gain, float bce_d_gain, int bce_zeroOffset, float bce_filter_freq, float bce_deadband) + configFileIO().saveBattData(batt_KP, batt_KI, batt_KD, batt_zero_offset, batt_frequency, batt_deadband); + xbee().printf("batt P:%6.2f, I:%6.2f, D:%6.2f, zero offset: %3i, limit %6.1f mm, slope %0.5f, filter_freq: %0.1f, deadband: %0.1f\r\n", batt().getControllerP(), batt().getControllerI(), batt().getControllerD(), batt().getZeroCounts(), batt().getTravelLimit(), batt().getPotSlope(), batt().getFilterFrequency(), batt().getDeadband()); } else if (BMM_PID_key == 'X') { break; //exit the while loop @@ -1792,13 +1781,15 @@ } else if (BMM_PID_key == 'F') { xbee().printf(">> Type in FILTER FREQUENCY with keyboard.\n\r"); - float batt_freq = getFloatUserInput(); - batt().setFilterFrequency(batt_freq); + batt_frequency = getFloatUserInput(); } else if (BMM_PID_key == 'B') { xbee().printf(">> Type in DEADBAND with keyboard.\n\r"); - float batt_deadband = getFloatUserInput(); - batt().setDeadband(batt_deadband); + batt_deadband = getFloatUserInput(); + } + else if (BMM_PID_key == 'Z') { + xbee().printf(">> Type in zero count offset with keyboard.\n\r"); + batt_zero_offset = (int)getFloatUserInput(); } else { xbee().printf("\n\rBATT: [%c] This key does nothing here. \r", BMM_PID_key); @@ -1812,6 +1803,7 @@ 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 + float depth_freq = depthLoop().getFilterFrequency(); float depth_deadband = depthLoop().getDeadband(); @@ -1837,6 +1829,7 @@ depthLoop().setControllerP(depth_KP); depthLoop().setControllerI(depth_KI); depthLoop().setControllerD(depth_KD); + depthLoop().setFilterFrequency(depth_freq); depthLoop().setDeadband(depth_deadband); @@ -1885,7 +1878,7 @@ 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 - float pitch_offset_deg = pitchLoop().getOutputOffset(); + float pitch_freq = pitchLoop().getFilterFrequency(); float pitch_deadband = pitchLoop().getDeadband(); @@ -1911,6 +1904,7 @@ pitchLoop().setControllerP(pitch_KP); pitchLoop().setControllerI(pitch_KI); pitchLoop().setControllerD(pitch_KD); + pitchLoop().setFilterFrequency(pitch_freq); pitchLoop().setDeadband(pitch_deadband); @@ -1946,9 +1940,6 @@ xbee().printf(">> Type in DEADBAND with keyboard.\n\r"); pitch_deadband = getFloatUserInput(); } - else if (PITCH_PID_key == 'O') { - pitch_offset_deg = getFloatUserInput(); - } else { xbee().printf("\n\rPITCH: [%c] This key does nothing here. \r", PITCH_PID_key); } @@ -1960,7 +1951,8 @@ float heading_KP = headingLoop().getControllerP(); float heading_KI = headingLoop().getControllerI(); - float heading_KD = headingLoop().getControllerD(); + float heading_KD = headingLoop().getControllerD(); + float heading_offset_deg = headingLoop().getOutputOffset(); float heading_freq = headingLoop().getFilterFrequency(); float heading_deadband = headingLoop().getDeadband();