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:
- 55:f4ec445c42fe
- Parent:
- 54:d4990fb68404
- Child:
- 56:48a8a5a65b82
--- a/StateMachine/StateMachine.cpp Fri Jun 08 13:56:30 2018 +0000 +++ b/StateMachine/StateMachine.cpp Tue Jun 12 15:41:51 2018 +0000 @@ -421,8 +421,8 @@ batt().unpause(); // what are the commands? - bce().setPosition_mm(_bceFloatPosition); - batt().setPosition_mm(_battFloatPosition); + bce().setPosition_mm(_bceFloatPosition); // 320.0 + batt().setPosition_mm(_battFloatPosition); // 73.0 //create the log file (works only if the file is closed) //createNewFile(); @@ -430,7 +430,7 @@ //show that this is the start of a new float broadcast sequence recordState(_state); - //triggers logger array + //triggers logger arrayc _is_log_timer_running = false; // reset the sub state timer to do one-shot actions again recordData(_state); } @@ -695,8 +695,8 @@ // output the keyboard menu for user's reference void StateMachine::showMenu() { - pc().printf("\r\r\n\nKEYBOARD MENU (02/15/2018):\r\r\n"); - pc().printf(" T to go into manual tuning mode (This is on a timer!)\r\n"); + pc().printf("\r\r\n\nKEYBOARD MENU (06/11/2018):\r\r\n"); + pc().printf(" T to go into CHECK TUNING (This is on a timer! Uses NEUTRAL positions!)\r\n"); pc().printf(" N to find neutral\r\n"); pc().printf(" M to initiate multi-dive cycle\r\n"); pc().printf(" D to initiate dive cycle\r\n"); @@ -725,6 +725,7 @@ pc().printf(" 6 STREAM STATUS\r\n"); pc().printf(" 7 RUDDER SERVO settings menu\r\n"); pc().printf(" 8 CHANNEL READINGS\r\n"); + pc().printf(" 9 POSITION READINGS\r\n"); pc().printf(" C See sensor readings (and max recorded depth of dive & neutral sequences)\r\n"); pc().printf(" ? to reset mbed\r\n"); @@ -1141,6 +1142,9 @@ else if (userInput == '8') { keyboard_menu_CHANNEL_READINGS(); } + else if (userInput == '9') { + keyboard_menu_POSITION_READINGS(); + } // go to sub-menus for the PID gains (this is blocking) else if (userInput == '1') { @@ -1220,7 +1224,7 @@ wait(1); - pc().printf("(set) BCE POS: %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS: %0.2f )(channel reading: %d) (volt: %0.2f) << %0.2f >> PRESSURE: %0.2f PITCH: %0.2f HEADING: %0.2f\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(), depthLoop().getPosition(),depth().readADCCounts(),depth().readVoltage(),depth().getRawPSI(),depth().getRawPSI(),imu().getPitch(),headingLoop().getPosition()); + pc().printf("(set) BCE POS: %0.1f (%0.1f) BATT POS: %0.1f (%0.1f) PRESS: %0.2f )(channel reading: %d) (volt: %0.2f) << %0.2f >> (Pressure: %0.2f (raw: %0.2f) PITCH: %0.2f HEADING: %0.2f)\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(),batt().getPosition_mm(), batt().getSetPosition_mm(), depthLoop().getPosition(),depth().readADCCounts(),depth().readVoltage(),depth().getPsi(),depth().getRawPSI(),depth().getRawPSI(),imu().getPitch(),imu().getHeading()); //pc().printf("Neutral Buoyancy Positions: bce: %0.1f, batt: %0.1f\r\n",_neutral_bce_pos_mm,_neutral_batt_pos_mm); @@ -1370,13 +1374,7 @@ 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 == '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 @@ -1412,7 +1410,35 @@ char TUNING_key; // show the menu - pc().printf("\n\r1: CHANNEL READINGS (EXIT WITH 'X' !)"); + pc().printf("\n\r8: CHANNEL READINGS (EXIT WITH 'X' !)"); + + while (1) { + if (pc().readable()) { + TUNING_key = pc().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 { + wait(0.5); + pc().printf("0(%d),1(%d),2(%d),6(%d),4(%d),5(%d),6(%d),7(%d)\r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4(),adc().readCh5(),adc().readCh6(),adc().readCh7()); + continue; // didn't get a user input, so keep waiting for it + } + } +} + +void StateMachine::keyboard_menu_POSITION_READINGS() { + char TUNING_key; + + // show the menu + pc().printf("\n\r9: BCE and BMM POSITION READINGS (EXIT WITH 'X' !)"); while (1) { if (pc().readable()) { @@ -1437,7 +1463,9 @@ vmeasured = ((float)raw)/4095.0*vref; - pc().printf("BCE POS(%d),BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d) \r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4()); + //pc().printf("BCE POS(%d),BMM POS(%d), BCE CUR(%d), BMM CUR(%d), Depth Pressure (%d) \r\n",adc().readCh0(),adc().readCh1(),adc().readCh2(),adc().readCh3(),adc().readCh4()); + + pc().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()); //pc().printf("vessel pressure(%d) batt voltage(%d) board current(%d) LIMIT: BCE(%d) BMM(%d) (HW reading limit: BCE(%d) BMM (%d) \r\n",adc().readCh5(),adc().readCh6(),adc().readCh7(),bce().getSwitch(), batt().getSwitch(),bce().getSwitchState(),batt().getSwitchState()); //pc().printf("LIMIT: BCE(%d) BMM(%d) << HW reading limit: BCE(%d) BMM (%d) >> \r\n",bce().getSwitch(), batt().getSwitch(),bce().getHWSwitchReading(),batt().getHWSwitchReading());