Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

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

main.cpp@11:3b241ecb75ed, 2017-10-27 (annotated)

Committer:
danstrider
Date:
Fri Oct 27 00:37:32 2017 +0000
Revision:
11:3b241ecb75ed
Parent:
10:085ab7328054
Child:
12:a0519d11d2b6
This version has been in the pool, working with all the hardware.  Had occasional string pot problems and got stuck in RISE, letting the battery stall out against the endcap.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danstrider 10:085ab7328054 1 /*
danstrider 10:085ab7328054 2 Starting from Trent's Linear Actuator code from 2017-10-19, these modifications
danstrider 10:085ab7328054 3 by Dan add an outer loop controller for depth and pitch to command the inner
danstrider 10:085ab7328054 4 linear actuator loops.
danstrider 10:085ab7328054 5 Modified 2017-10-20 revA by Dan
danstrider 10:085ab7328054 6 - added outer loop controller, but it is hanging the mbed. (turns out it was the imu update)
danstrider 10:085ab7328054 7 Modified 2017-10-22 revA by Dan
danstrider 10:085ab7328054 8 - outer loop now works with a call to outerloop.update() in main loop(), not with an attached ticker
danstrider 10:085ab7328054 9 Modified 2017-10-22 revB by Dan
danstrider 10:085ab7328054 10 - enabled both depth and pitch outer loop controllers
danstrider 10:085ab7328054 11 - added ability to keyboard reset
danstrider 10:085ab7328054 12 Modified 2017-10-22 revC by Dan
danstrider 10:085ab7328054 13 - major update to the IMU library processing to make a state machine that doesn't hang
danstrider 10:085ab7328054 14 - added lat/lon/alt and GNSS fix information to the IMU library
danstrider 10:085ab7328054 15 - brought out the pin names into the constructors of IMU, omega, SpiADC
danstrider 10:085ab7328054 16 Modified 2017-10-22 revD by Dan
danstrider 10:085ab7328054 17 - everything seems to be working, shy of re-checking on the hardware
danstrider 10:085ab7328054 18 - Depth sensor call done inside the OuterLoop, but should somehow be set as a callback instead
danstrider 10:085ab7328054 19 - IMU sensor call done inside the OuterLoop, but should somehow be set as a callback instead
danstrider 10:085ab7328054 20 Modified 2017-10-23 revA by Dan/Matt
danstrider 10:085ab7328054 21 - linear actuator hardware works great, limit switches, sensing, etc.
danstrider 10:085ab7328054 22 - outer loops run, but move the BCE in the wrong direction.
danstrider 10:085ab7328054 23 - new IMU code doesn't read from the sensor correctly, but doesn't hang up either.
danstrider 10:085ab7328054 24 - depth sensor worked fine, just needs zero bias adjustment.
danstrider 11:3b241ecb75ed 25 Modified 2017-10-24 by Troy
danstrider 11:3b241ecb75ed 26 - added offset to outerloop
danstrider 11:3b241ecb75ed 27 Modified 2017-10-26 by Dan
danstrider 11:3b241ecb75ed 28 - brought over a state machine and new keyboard controls ... currently just dumped into main.
danstrider 11:3b241ecb75ed 29 Modified 2017-10-26 by Matt
danstrider 11:3b241ecb75ed 30 - new IMU code imported and working well with the hardware.
danstrider 11:3b241ecb75ed 31 Modified 2017-10-26 revB by Dan
danstrider 11:3b241ecb75ed 32 - This version has been in the pool.
danstrider 11:3b241ecb75ed 33 - Get occasional ADC bad string pot batt & piston. On initial sensor reads, see negative positions.
danstrider 11:3b241ecb75ed 34 But after running FLOAT_BROADCAST, string pot positions are normal. Not sure why.
danstrider 11:3b241ecb75ed 35 - Repeatedly got stuck in RISE with a +60s timeout. Battery hit end bell and stalled out.
danstrider 11:3b241ecb75ed 36 - keyboard and state machine are in main, probably shouldn't be, but easier to debug.
danstrider 11:3b241ecb75ed 37 - Really happy with the logic and flow of the state machine. Timeouts work.
danstrider 11:3b241ecb75ed 38 - Need to add a means to drive the linear actuators manually ... toggle out stop() in SIT_IDLE.
danstrider 11:3b241ecb75ed 39 - Need to add keyboard commands to modify the zeroOffset positions.
danstrider 10:085ab7328054 40 */
danstrider 10:085ab7328054 41
tzyoung 0:ea293bbf9717 42 #include "mbed.h"
tzyoung 0:ea293bbf9717 43 #include "StaticDefs.hpp"
tzyoung 2:892b58e56712 44 #include "config_functions.h"
tzyoung 2:892b58e56712 45
danstrider 11:3b241ecb75ed 46
danstrider 10:085ab7328054 47 extern "C" void mbed_reset(); // utilized to reset the mbed
tzyoung 4:66f13fbb035d 48
danstrider 11:3b241ecb75ed 49 // state enumerations
danstrider 11:3b241ecb75ed 50 enum {
danstrider 11:3b241ecb75ed 51 SIT_IDLE, // stops both motors, exits after a keyboard input
danstrider 11:3b241ecb75ed 52 KEYBOARD, // handles an individual keypress, exits to state by a keyboard menu
danstrider 11:3b241ecb75ed 53 FIND_NEUTRAL, // dives to depth at zero pitch, exits when stable
danstrider 11:3b241ecb75ed 54 DIVE, // dives to depth at negative pitch, exits when crossing a defined depth
danstrider 11:3b241ecb75ed 55 RISE, // rises to surface at positive pitch, exits when near surface
danstrider 11:3b241ecb75ed 56 FLOAT_LEVEL, // bce position to float, pitch loop active at zero, exits when stable near zero pitch
danstrider 11:3b241ecb75ed 57 FLOAT_BROADCAST, // bce position to float, batt position forward to hold tail up, exits when actuators done
danstrider 11:3b241ecb75ed 58 EMERGENCY_CLIMB // bce position to full rise, batt position to full aft, exits when at surface
danstrider 11:3b241ecb75ed 59 };
danstrider 11:3b241ecb75ed 60
danstrider 11:3b241ecb75ed 61 // timer for the state machine
danstrider 11:3b241ecb75ed 62 Timer timer;
danstrider 11:3b241ecb75ed 63
danstrider 11:3b241ecb75ed 64 // these are all the parameters needed for the state machine
danstrider 11:3b241ecb75ed 65 int timeout = 60; // generic timeout for every state, seconds
danstrider 11:3b241ecb75ed 66 float depthTolerance = 0.25; // depth tolerance for neutral finding exit critera
danstrider 11:3b241ecb75ed 67 float pitchTolerance = 1.0; // pitch angle tolerance for neutral finding exit criteria
danstrider 11:3b241ecb75ed 68 float bceFloatPosition = 300; // bce position for "float" states
danstrider 11:3b241ecb75ed 69 float battFloatPosition = 50; // batt position for "broadcast" state
danstrider 11:3b241ecb75ed 70 float depthCommand = 3.5; // user keyboard depth
danstrider 11:3b241ecb75ed 71 float pitchCommand = -5.0; // user keyboard depth
danstrider 11:3b241ecb75ed 72
danstrider 11:3b241ecb75ed 73
danstrider 10:085ab7328054 74 void setup() {
danstrider 11:3b241ecb75ed 75 pc().printf("\n\n\r\rFSG 2017-10-26 revA\n\r");
danstrider 11:3b241ecb75ed 76 pc().baud(57600);
tzyoung 4:66f13fbb035d 77
danstrider 10:085ab7328054 78 // start up the system timer
tzyoung 0:ea293bbf9717 79 systemTime().start();
tzyoung 0:ea293bbf9717 80
danstrider 10:085ab7328054 81 // set up and start the adc. This runs on a fixed interval and is interrupt driven
tzyoung 0:ea293bbf9717 82 adc().initialize();
tzyoung 0:ea293bbf9717 83 adc().start();
danstrider 10:085ab7328054 84
danstrider 10:085ab7328054 85 // set up and start the imu. This polls in the background
danstrider 10:085ab7328054 86 imu().initialize();
danstrider 10:085ab7328054 87 imu().start();
danstrider 10:085ab7328054 88
danstrider 10:085ab7328054 89 // set up the depth sensor. This is an internal ADC read, but eventually will be on the ltc1298
danstrider 10:085ab7328054 90 depth().initialize();
danstrider 10:085ab7328054 91
danstrider 10:085ab7328054 92 // construct a local file system
danstrider 10:085ab7328054 93 local();
tzyoung 0:ea293bbf9717 94
danstrider 10:085ab7328054 95 // load config data from files
danstrider 10:085ab7328054 96 load_BCE_config(); // load the buoyancy engine parameters from the file "bce.txt"
danstrider 10:085ab7328054 97 load_BATT_config(); // load the battery mass mover parameters from the file "batt.txt"
danstrider 10:085ab7328054 98 load_DEPTH_config(); // load the depth control loop parameters from the file "depth.txt"
danstrider 10:085ab7328054 99 load_PITCH_config(); // load the depth control loop parameters from the file "pitch.txt"
tzyoung 4:66f13fbb035d 100
danstrider 10:085ab7328054 101 // set up the linear actuators. adc has to be running first.
danstrider 10:085ab7328054 102 bce().init();
danstrider 10:085ab7328054 103 bce().start();
danstrider 10:085ab7328054 104 bce().setPosition_mm(bce().getPosition_mm()); // start by not moving
tzyoung 4:66f13fbb035d 105
danstrider 10:085ab7328054 106 batt().init();
tzyoung 2:892b58e56712 107 batt().start();
danstrider 10:085ab7328054 108 batt().setPosition_mm(batt().getPosition_mm()); // start by not moving
danstrider 10:085ab7328054 109
danstrider 10:085ab7328054 110 // set up the depth and pitch outer loop controllers
danstrider 10:085ab7328054 111 depthLoop().init();
danstrider 11:3b241ecb75ed 112 depthLoop().setCommand(-2.5);
mkelly10 5:15bd96205bb2 113
danstrider 10:085ab7328054 114 pitchLoop().init();
danstrider 10:085ab7328054 115 pitchLoop().setCommand(0.0);
danstrider 10:085ab7328054 116
danstrider 10:085ab7328054 117 // do not leave this in. Check that PID gains are loading
danstrider 11:3b241ecb75ed 118 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());
danstrider 11:3b241ecb75ed 119 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());
danstrider 11:3b241ecb75ed 120 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());
danstrider 11:3b241ecb75ed 121 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());
danstrider 10:085ab7328054 122 pc().printf("\n\r");
danstrider 10:085ab7328054 123 }
mkelly10 5:15bd96205bb2 124
danstrider 11:3b241ecb75ed 125
danstrider 11:3b241ecb75ed 126
danstrider 11:3b241ecb75ed 127 void keyboard_menu_BCE_PID_settings() {
danstrider 11:3b241ecb75ed 128 char PID_key;
danstrider 11:3b241ecb75ed 129 float gain_step_size = 0.01; // modify this to change gain step size
danstrider 11:3b241ecb75ed 130 float KP = bce().getControllerP(); // load current value
danstrider 11:3b241ecb75ed 131 float KI = bce().getControllerI(); // load current global value
danstrider 11:3b241ecb75ed 132 float KD = bce().getControllerD(); // load current global value
danstrider 11:3b241ecb75ed 133
danstrider 11:3b241ecb75ed 134 // show the menu
danstrider 11:3b241ecb75ed 135 pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
danstrider 11:3b241ecb75ed 136 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
danstrider 11:3b241ecb75ed 137 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
danstrider 11:3b241ecb75ed 138 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());
danstrider 11:3b241ecb75ed 139
danstrider 11:3b241ecb75ed 140 // handle the key presses
danstrider 11:3b241ecb75ed 141 while(1) {
danstrider 11:3b241ecb75ed 142 // get the user's keystroke from either of the two inputs
danstrider 11:3b241ecb75ed 143 if (pc().readable()) {
danstrider 11:3b241ecb75ed 144 PID_key = pc().getc();
danstrider 11:3b241ecb75ed 145 }
danstrider 11:3b241ecb75ed 146 else {
danstrider 11:3b241ecb75ed 147 continue; // didn't get a user input, so keep waiting for it
danstrider 11:3b241ecb75ed 148 }
danstrider 11:3b241ecb75ed 149
danstrider 11:3b241ecb75ed 150 // handle the user's key input
danstrider 11:3b241ecb75ed 151 if (PID_key == '-') {
danstrider 11:3b241ecb75ed 152 KP -= gain_step_size;
danstrider 11:3b241ecb75ed 153 pc().printf("P gain: %0.5f \r\n", KP);
danstrider 11:3b241ecb75ed 154 }
danstrider 11:3b241ecb75ed 155 else if (PID_key == '=') {
danstrider 11:3b241ecb75ed 156 KP += gain_step_size;
danstrider 11:3b241ecb75ed 157 pc().printf("P gain: %0.5f \r\n", KP);
danstrider 11:3b241ecb75ed 158 }
danstrider 11:3b241ecb75ed 159 else if (PID_key == '[') {
danstrider 11:3b241ecb75ed 160 KI -= gain_step_size;
danstrider 11:3b241ecb75ed 161 pc().printf("I gain: %0.5f \r\n", KI);
danstrider 11:3b241ecb75ed 162 }
danstrider 11:3b241ecb75ed 163 else if (PID_key == ']') {
danstrider 11:3b241ecb75ed 164 KI += gain_step_size;
danstrider 11:3b241ecb75ed 165 pc().printf("I gain: %0.5f \r\n", KI);
danstrider 11:3b241ecb75ed 166 }
danstrider 11:3b241ecb75ed 167 else if (PID_key == ';') {
danstrider 11:3b241ecb75ed 168 KD -= gain_step_size;
danstrider 11:3b241ecb75ed 169 pc().printf("D gain: %0.5f \r\n", KD);
danstrider 11:3b241ecb75ed 170 }
danstrider 11:3b241ecb75ed 171 else if (PID_key == '\'') {
danstrider 11:3b241ecb75ed 172 KD += gain_step_size;
danstrider 11:3b241ecb75ed 173 pc().printf("D gain: %0.5f \r\n", KD);
danstrider 11:3b241ecb75ed 174 }
danstrider 11:3b241ecb75ed 175 else if (PID_key == 'S') { // user wants to save these modified values
danstrider 11:3b241ecb75ed 176 // set values
danstrider 11:3b241ecb75ed 177 bce().setControllerP(KP);
danstrider 11:3b241ecb75ed 178 bce().setControllerI(KI);
danstrider 11:3b241ecb75ed 179 bce().setControllerD(KD);
danstrider 11:3b241ecb75ed 180
danstrider 11:3b241ecb75ed 181 // save into "PID.cfg"
danstrider 11:3b241ecb75ed 182 //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);
danstrider 11:3b241ecb75ed 183 break; //exit the while loop
danstrider 11:3b241ecb75ed 184 }
danstrider 11:3b241ecb75ed 185 else if (PID_key == 'X') {
danstrider 11:3b241ecb75ed 186 break; //exit the while loop
danstrider 11:3b241ecb75ed 187 }
danstrider 11:3b241ecb75ed 188 else {
danstrider 11:3b241ecb75ed 189 pc().printf("\n\rThis key does nothing here. ");
danstrider 11:3b241ecb75ed 190 }
danstrider 11:3b241ecb75ed 191 }
danstrider 11:3b241ecb75ed 192 }
danstrider 11:3b241ecb75ed 193
danstrider 11:3b241ecb75ed 194 void keyboard_menu_DEPTH_PID_settings() {
danstrider 11:3b241ecb75ed 195 char PID_key;
danstrider 11:3b241ecb75ed 196 float gain_step_size = 0.01; // modify this to change gain step size
danstrider 11:3b241ecb75ed 197 float KP = depthLoop().getControllerP(); // load current global value
danstrider 11:3b241ecb75ed 198 float KI = depthLoop().getControllerI(); // load current global value
danstrider 11:3b241ecb75ed 199 float KD = depthLoop().getControllerD(); // load current global value
danstrider 11:3b241ecb75ed 200
danstrider 11:3b241ecb75ed 201 // show the menu
danstrider 11:3b241ecb75ed 202 pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
danstrider 11:3b241ecb75ed 203 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
danstrider 11:3b241ecb75ed 204 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\n\n\r");
danstrider 11:3b241ecb75ed 205 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());
danstrider 11:3b241ecb75ed 206
danstrider 11:3b241ecb75ed 207 // handle the key presses
danstrider 11:3b241ecb75ed 208 while(1) {
danstrider 11:3b241ecb75ed 209 // get the user's keystroke from either of the two inputs
danstrider 11:3b241ecb75ed 210 if (pc().readable()) {
danstrider 11:3b241ecb75ed 211 PID_key = pc().getc();
danstrider 11:3b241ecb75ed 212 }
danstrider 11:3b241ecb75ed 213 else {
danstrider 11:3b241ecb75ed 214 continue; // didn't get a user input, so keep waiting for it
danstrider 11:3b241ecb75ed 215 }
danstrider 11:3b241ecb75ed 216
danstrider 11:3b241ecb75ed 217 // handle the user's key input
danstrider 11:3b241ecb75ed 218 if (PID_key == '-') {
danstrider 11:3b241ecb75ed 219 KP -= gain_step_size;
danstrider 11:3b241ecb75ed 220 pc().printf("P gain: %0.5f \r\n", KP);
danstrider 11:3b241ecb75ed 221 }
danstrider 11:3b241ecb75ed 222 else if (PID_key == '=') {
danstrider 11:3b241ecb75ed 223 KP += gain_step_size;
danstrider 11:3b241ecb75ed 224 pc().printf("P gain: %0.5f \r\n", KP);
danstrider 11:3b241ecb75ed 225 }
danstrider 11:3b241ecb75ed 226 else if (PID_key == '[') {
danstrider 11:3b241ecb75ed 227 KI -= gain_step_size;
danstrider 11:3b241ecb75ed 228 pc().printf("I gain: %0.5f \r\n", KI);
danstrider 11:3b241ecb75ed 229 }
danstrider 11:3b241ecb75ed 230 else if (PID_key == ']') {
danstrider 11:3b241ecb75ed 231 KI += gain_step_size;
danstrider 11:3b241ecb75ed 232 pc().printf("I gain: %0.5f \r\n", KI);
danstrider 11:3b241ecb75ed 233 }
danstrider 11:3b241ecb75ed 234 else if (PID_key == ';') {
danstrider 11:3b241ecb75ed 235 KD -= gain_step_size;
danstrider 11:3b241ecb75ed 236 pc().printf("D gain: %0.5f \r\n", KD);
danstrider 11:3b241ecb75ed 237 }
danstrider 11:3b241ecb75ed 238 else if (PID_key == '\'') {
danstrider 11:3b241ecb75ed 239 KD += gain_step_size;
danstrider 11:3b241ecb75ed 240 pc().printf("D gain: %0.5f \r\n", KD);
danstrider 11:3b241ecb75ed 241 }
danstrider 11:3b241ecb75ed 242 else if (PID_key == 'S') { // user wants to save these settings
danstrider 11:3b241ecb75ed 243 // set global values
danstrider 11:3b241ecb75ed 244 depthLoop().setControllerP(KP);
danstrider 11:3b241ecb75ed 245 depthLoop().setControllerI(KI);
danstrider 11:3b241ecb75ed 246 depthLoop().setControllerD(KD);
danstrider 11:3b241ecb75ed 247
danstrider 11:3b241ecb75ed 248 // save into "PID.cfg"
danstrider 11:3b241ecb75ed 249 //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);
danstrider 11:3b241ecb75ed 250 break; //exit the while loop
danstrider 11:3b241ecb75ed 251 }
danstrider 11:3b241ecb75ed 252 else if (PID_key == 'X') {
danstrider 11:3b241ecb75ed 253 break; //exit the while loop
danstrider 11:3b241ecb75ed 254 }
danstrider 11:3b241ecb75ed 255 else {
danstrider 11:3b241ecb75ed 256 pc().printf("\n\rThis key does nothing here. ");
danstrider 11:3b241ecb75ed 257 }
danstrider 11:3b241ecb75ed 258 }
danstrider 11:3b241ecb75ed 259 }
danstrider 11:3b241ecb75ed 260
danstrider 11:3b241ecb75ed 261 void keyboard_menu_BATT_PID_settings() {
danstrider 11:3b241ecb75ed 262 char PID_key;
danstrider 11:3b241ecb75ed 263 float gain_step_size = 0.01; // modify this to change gain step size
danstrider 11:3b241ecb75ed 264 float KP = batt().getControllerP(); // load current global value
danstrider 11:3b241ecb75ed 265 float KI = batt().getControllerI(); // load current global value
danstrider 11:3b241ecb75ed 266 float KD = batt().getControllerD(); // load current global value
danstrider 11:3b241ecb75ed 267
danstrider 11:3b241ecb75ed 268 // print the menu
danstrider 11:3b241ecb75ed 269 pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
danstrider 11:3b241ecb75ed 270 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
danstrider 11:3b241ecb75ed 271 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
danstrider 11:3b241ecb75ed 272 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());
danstrider 11:3b241ecb75ed 273
danstrider 11:3b241ecb75ed 274 // handle the key presses
danstrider 11:3b241ecb75ed 275 while(1) {
danstrider 11:3b241ecb75ed 276 // get the user's keystroke from either of the two inputs
danstrider 11:3b241ecb75ed 277 if (pc().readable()) {
danstrider 11:3b241ecb75ed 278 PID_key = pc().getc();
danstrider 11:3b241ecb75ed 279 }
danstrider 11:3b241ecb75ed 280 else {
danstrider 11:3b241ecb75ed 281 continue; // didn't get a user input, so keep waiting for it
danstrider 11:3b241ecb75ed 282 }
danstrider 11:3b241ecb75ed 283
danstrider 11:3b241ecb75ed 284 // handle the user's key input
danstrider 11:3b241ecb75ed 285 if (PID_key == '-') {
danstrider 11:3b241ecb75ed 286 KP -= gain_step_size;
danstrider 11:3b241ecb75ed 287 pc().printf("\rP gain: %0.5f ", KP);
danstrider 11:3b241ecb75ed 288 }
danstrider 11:3b241ecb75ed 289 else if (PID_key == '=') {
danstrider 11:3b241ecb75ed 290 KP += gain_step_size;
danstrider 11:3b241ecb75ed 291 pc().printf("\rP gain: %0.5f ", KP);
danstrider 11:3b241ecb75ed 292 }
danstrider 11:3b241ecb75ed 293 else if (PID_key == '[') {
danstrider 11:3b241ecb75ed 294 KI -= gain_step_size;
danstrider 11:3b241ecb75ed 295 pc().printf("\rI gain: %0.5f ", KI);
danstrider 11:3b241ecb75ed 296 }
danstrider 11:3b241ecb75ed 297 else if (PID_key == ']') {
danstrider 11:3b241ecb75ed 298 KI += gain_step_size;
danstrider 11:3b241ecb75ed 299 pc().printf("\rI gain: %0.5f ", KI);
danstrider 11:3b241ecb75ed 300 }
danstrider 11:3b241ecb75ed 301 else if (PID_key == ';') {
danstrider 11:3b241ecb75ed 302 KD -= gain_step_size;
danstrider 11:3b241ecb75ed 303 pc().printf("\rD gain: %0.5f ", KD);
danstrider 11:3b241ecb75ed 304 }
danstrider 11:3b241ecb75ed 305 else if (PID_key == '\'') {
danstrider 11:3b241ecb75ed 306 KD += gain_step_size;
danstrider 11:3b241ecb75ed 307 pc().printf("\rD gain: %0.5f ", KD);
danstrider 11:3b241ecb75ed 308 }
danstrider 11:3b241ecb75ed 309 else if (PID_key == 'S') { // user wants to save the modified values
danstrider 11:3b241ecb75ed 310 // set global values
danstrider 11:3b241ecb75ed 311 batt().setControllerP(KP);
danstrider 11:3b241ecb75ed 312 batt().setControllerI(KI);
danstrider 11:3b241ecb75ed 313 batt().setControllerD(KD);
danstrider 11:3b241ecb75ed 314
danstrider 11:3b241ecb75ed 315 // save to "PID.cfg" file
danstrider 11:3b241ecb75ed 316 //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);
danstrider 11:3b241ecb75ed 317 break; //exit the while loop
danstrider 11:3b241ecb75ed 318 }
danstrider 11:3b241ecb75ed 319 else if (PID_key == 'X') {
danstrider 11:3b241ecb75ed 320 break; //exit the while loop
danstrider 11:3b241ecb75ed 321 }
danstrider 11:3b241ecb75ed 322 else {
danstrider 11:3b241ecb75ed 323 pc().printf("This key does nothing here.\r");
danstrider 11:3b241ecb75ed 324 }
danstrider 11:3b241ecb75ed 325 }
danstrider 11:3b241ecb75ed 326 }
danstrider 11:3b241ecb75ed 327
danstrider 11:3b241ecb75ed 328 void keyboard_menu_PITCH_PID_settings() {
danstrider 11:3b241ecb75ed 329 char PID_key;
danstrider 11:3b241ecb75ed 330 float gain_step_size = 0.01; // modify this to change gain step size
danstrider 11:3b241ecb75ed 331 float KP = pitchLoop().getControllerP(); // load current global value
danstrider 11:3b241ecb75ed 332 float KI = pitchLoop().getControllerI(); // load current global value
danstrider 11:3b241ecb75ed 333 float KD = pitchLoop().getControllerD(); // load current global value
danstrider 11:3b241ecb75ed 334
danstrider 11:3b241ecb75ed 335 // print the menu
danstrider 11:3b241ecb75ed 336 pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
danstrider 11:3b241ecb75ed 337 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
danstrider 11:3b241ecb75ed 338 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
danstrider 11:3b241ecb75ed 339 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());
danstrider 11:3b241ecb75ed 340
danstrider 11:3b241ecb75ed 341 // handle the key presses
danstrider 11:3b241ecb75ed 342 while(1) {
danstrider 11:3b241ecb75ed 343 // get the user's keystroke from either of the two inputs
danstrider 11:3b241ecb75ed 344 if (pc().readable()) {
danstrider 11:3b241ecb75ed 345 PID_key = pc().getc();
danstrider 11:3b241ecb75ed 346 }
danstrider 11:3b241ecb75ed 347 else {
danstrider 11:3b241ecb75ed 348 continue; // didn't get a user input, so keep waiting for it
danstrider 11:3b241ecb75ed 349 }
danstrider 11:3b241ecb75ed 350
danstrider 11:3b241ecb75ed 351 // handle the user's key input
danstrider 11:3b241ecb75ed 352 if (PID_key == '-') {
danstrider 11:3b241ecb75ed 353 KP -= gain_step_size;
danstrider 11:3b241ecb75ed 354 pc().printf("\rP gain: %0.5f ", KP);
danstrider 11:3b241ecb75ed 355 }
danstrider 11:3b241ecb75ed 356 else if (PID_key == '=') {
danstrider 11:3b241ecb75ed 357 KP += gain_step_size;
danstrider 11:3b241ecb75ed 358 pc().printf("\rP gain: %0.5f ", KP);
danstrider 11:3b241ecb75ed 359 }
danstrider 11:3b241ecb75ed 360 else if (PID_key == '[') {
danstrider 11:3b241ecb75ed 361 KI -= gain_step_size;
danstrider 11:3b241ecb75ed 362 pc().printf("\rI gain: %0.5f ", KI);
danstrider 11:3b241ecb75ed 363 }
danstrider 11:3b241ecb75ed 364 else if (PID_key == ']') {
danstrider 11:3b241ecb75ed 365 KI += gain_step_size;
danstrider 11:3b241ecb75ed 366 pc().printf("\rI gain: %0.5f ", KI);
danstrider 11:3b241ecb75ed 367 }
danstrider 11:3b241ecb75ed 368 else if (PID_key == ';') {
danstrider 11:3b241ecb75ed 369 KD -= gain_step_size;
danstrider 11:3b241ecb75ed 370 pc().printf("\rD gain: %0.5f ", KD);
danstrider 11:3b241ecb75ed 371 }
danstrider 11:3b241ecb75ed 372 else if (PID_key == '\'') {
danstrider 11:3b241ecb75ed 373 KD += gain_step_size;
danstrider 11:3b241ecb75ed 374 pc().printf("\rD gain: %0.5f ", KD);
danstrider 11:3b241ecb75ed 375 }
danstrider 11:3b241ecb75ed 376 else if (PID_key == 'S') { // user wants to save the modified values
danstrider 11:3b241ecb75ed 377 // set global values
danstrider 11:3b241ecb75ed 378 pitchLoop().setControllerP(KP);
danstrider 11:3b241ecb75ed 379 pitchLoop().setControllerI(KI);
danstrider 11:3b241ecb75ed 380 pitchLoop().setControllerD(KD);
danstrider 11:3b241ecb75ed 381
danstrider 11:3b241ecb75ed 382 //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);
danstrider 11:3b241ecb75ed 383 break; //exit the while loop
danstrider 11:3b241ecb75ed 384 }
danstrider 11:3b241ecb75ed 385 else if (PID_key == 'X') {
danstrider 11:3b241ecb75ed 386 break; //exit the while loop
danstrider 11:3b241ecb75ed 387 }
danstrider 11:3b241ecb75ed 388 else {
danstrider 11:3b241ecb75ed 389 pc().printf("This key does nothing here.\r");
danstrider 11:3b241ecb75ed 390 }
danstrider 11:3b241ecb75ed 391 }
danstrider 11:3b241ecb75ed 392 }
danstrider 11:3b241ecb75ed 393
danstrider 11:3b241ecb75ed 394
danstrider 11:3b241ecb75ed 395 // output the keyboard menu for user's reference
danstrider 11:3b241ecb75ed 396 void showMenu() {
danstrider 11:3b241ecb75ed 397 pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n");
danstrider 11:3b241ecb75ed 398 pc().printf(" N to find neutral\r\n");
danstrider 11:3b241ecb75ed 399 pc().printf(" D to initiate dive cycle\r\n");
danstrider 11:3b241ecb75ed 400 pc().printf(" R to initiate rise\r\n");
danstrider 11:3b241ecb75ed 401 pc().printf(" L to float level\r\n");
danstrider 11:3b241ecb75ed 402 pc().printf(" B to float at broadcast pitch\r\n");
danstrider 11:3b241ecb75ed 403 pc().printf(" E to initiate emergency climb\r\n");
danstrider 11:3b241ecb75ed 404 //pc().printf(" H to run homing sequence on both BCE and Batt\r\n");
danstrider 11:3b241ecb75ed 405 pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 406 pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",depthLoop().getCommand());
danstrider 11:3b241ecb75ed 407 pc().printf("+/- to decrease/increase timeout: %d s\r\n",timeout);
danstrider 11:3b241ecb75ed 408 pc().printf(" 1 BCE PID sub-menu\r\n");
danstrider 11:3b241ecb75ed 409 pc().printf(" 2 BATT PID sub-menu\r\n");
danstrider 11:3b241ecb75ed 410 pc().printf(" 3 Depth PID sub-menu\r\n");
danstrider 11:3b241ecb75ed 411 pc().printf(" 4 Pitch PID sub-menu\r\n");
danstrider 11:3b241ecb75ed 412 pc().printf(" C See sensor readings\r\n");
danstrider 11:3b241ecb75ed 413 pc().printf(" ? to reset mbed\r\n");
danstrider 11:3b241ecb75ed 414 }
danstrider 11:3b241ecb75ed 415
danstrider 11:3b241ecb75ed 416 // keyboard currently handles a key at a time
danstrider 11:3b241ecb75ed 417 // returns -1 if not a state command
danstrider 11:3b241ecb75ed 418 // returns a positive number to command a new state
danstrider 11:3b241ecb75ed 419 int keyboard() {
danstrider 10:085ab7328054 420 char userInput;
danstrider 10:085ab7328054 421
danstrider 10:085ab7328054 422 // check keyboard and make settings changes as requested
danstrider 10:085ab7328054 423 if (pc().readable()) {
danstrider 10:085ab7328054 424 // get the key
danstrider 10:085ab7328054 425 userInput = pc().getc();
mkelly10 5:15bd96205bb2 426
danstrider 10:085ab7328054 427 // check command against desired control buttons
danstrider 11:3b241ecb75ed 428 // change state
danstrider 11:3b241ecb75ed 429 if (userInput == 'D' or userInput == 'd') {
danstrider 11:3b241ecb75ed 430 return DIVE;
danstrider 10:085ab7328054 431 }
danstrider 11:3b241ecb75ed 432 else if (userInput == 'N' or userInput == 'n') {
danstrider 11:3b241ecb75ed 433 return FIND_NEUTRAL;
danstrider 11:3b241ecb75ed 434 }
danstrider 11:3b241ecb75ed 435 else if (userInput == 'R' or userInput == 'r') {
danstrider 11:3b241ecb75ed 436 return RISE;
danstrider 11:3b241ecb75ed 437 }
danstrider 11:3b241ecb75ed 438 else if (userInput == 'L' or userInput == 'l') {
danstrider 11:3b241ecb75ed 439 return FLOAT_LEVEL;
danstrider 10:085ab7328054 440 }
danstrider 11:3b241ecb75ed 441 else if (userInput == 'B' or userInput == 'b') {
danstrider 11:3b241ecb75ed 442 return FLOAT_BROADCAST;
danstrider 11:3b241ecb75ed 443 }
danstrider 11:3b241ecb75ed 444 else if (userInput == 'E' or userInput == 'e') {
danstrider 11:3b241ecb75ed 445 return EMERGENCY_CLIMB;
mkelly10 5:15bd96205bb2 446 }
danstrider 11:3b241ecb75ed 447 // else if (userInput == 'H' or userInput == 'h') {
danstrider 11:3b241ecb75ed 448 // pc().printf("running homing procedure\r\n");
danstrider 11:3b241ecb75ed 449 // bce().start(); bce().homePiston(); bce().stop();
danstrider 11:3b241ecb75ed 450 // batt().start(); batt().homePiston(); batt().stop();
danstrider 11:3b241ecb75ed 451 // return SIT_IDLE;
danstrider 11:3b241ecb75ed 452 // }
danstrider 10:085ab7328054 453 else if (userInput == '?') {
danstrider 10:085ab7328054 454 pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
danstrider 11:3b241ecb75ed 455 wait(0.5);
danstrider 10:085ab7328054 456 mbed_reset();
danstrider 10:085ab7328054 457 }
danstrider 11:3b241ecb75ed 458
danstrider 11:3b241ecb75ed 459 // change settings
danstrider 11:3b241ecb75ed 460 else if (userInput == 'Q' or userInput == 'q') {
danstrider 11:3b241ecb75ed 461 pitchCommand -= 0.5; //decrement the pitch setpoint
danstrider 11:3b241ecb75ed 462 pitchLoop().setCommand(pitchCommand);
danstrider 11:3b241ecb75ed 463 pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 464 }
danstrider 11:3b241ecb75ed 465 else if (userInput == 'W' or userInput == 'w') {
danstrider 11:3b241ecb75ed 466 pitchCommand += 0.5; //increment the pitch setpoint
danstrider 11:3b241ecb75ed 467 pitchLoop().setCommand(pitchCommand);
danstrider 11:3b241ecb75ed 468 pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 469 }
danstrider 11:3b241ecb75ed 470 else if (userInput == 'A' or userInput == 'a') {
danstrider 11:3b241ecb75ed 471 depthCommand -= 0.5; //decrement the depth setpoint
danstrider 11:3b241ecb75ed 472 depthLoop().setCommand(depthCommand);
danstrider 11:3b241ecb75ed 473 pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand());
danstrider 11:3b241ecb75ed 474 }
danstrider 11:3b241ecb75ed 475 else if (userInput == 'S' or userInput == 's') {
danstrider 11:3b241ecb75ed 476 depthCommand += 0.5; //increment the depth setpoint
danstrider 11:3b241ecb75ed 477 depthLoop().setCommand(depthCommand);
danstrider 11:3b241ecb75ed 478 pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
danstrider 11:3b241ecb75ed 479 }
danstrider 11:3b241ecb75ed 480 else if (userInput == '-') {
danstrider 11:3b241ecb75ed 481 timeout -= 10.0; //decrement the timeout
danstrider 11:3b241ecb75ed 482 pc().printf(">>> timeout decreased: %d\r\n", timeout);
danstrider 11:3b241ecb75ed 483 }
danstrider 11:3b241ecb75ed 484 else if (userInput == '=' or userInput == '+') {
danstrider 11:3b241ecb75ed 485 timeout += 10.0; //increment the timeout
danstrider 11:3b241ecb75ed 486 pc().printf(">>> timeout increased: %d\r\n", timeout);
danstrider 11:3b241ecb75ed 487 }
danstrider 11:3b241ecb75ed 488
danstrider 11:3b241ecb75ed 489 // add keyboard commands to move the neutral zero offsets, both bce and batt
danstrider 11:3b241ecb75ed 490
danstrider 11:3b241ecb75ed 491 // go to sub-menus for the PID gains (this is blocking)
danstrider 11:3b241ecb75ed 492 else if (userInput == '1') {
danstrider 11:3b241ecb75ed 493 keyboard_menu_BCE_PID_settings();
danstrider 11:3b241ecb75ed 494 }
danstrider 11:3b241ecb75ed 495 else if (userInput == '2') {
danstrider 11:3b241ecb75ed 496 keyboard_menu_BATT_PID_settings();
danstrider 11:3b241ecb75ed 497 }
danstrider 11:3b241ecb75ed 498 else if (userInput == '3') {
danstrider 11:3b241ecb75ed 499 keyboard_menu_DEPTH_PID_settings();
danstrider 11:3b241ecb75ed 500 }
danstrider 11:3b241ecb75ed 501 else if (userInput == '4') {
danstrider 11:3b241ecb75ed 502 keyboard_menu_PITCH_PID_settings();
danstrider 11:3b241ecb75ed 503 }
danstrider 11:3b241ecb75ed 504
danstrider 11:3b241ecb75ed 505 else if (userInput == 'C' or userInput == 'c') {
danstrider 11:3b241ecb75ed 506 pc().printf("depth: %3.1f\r\n",depth().getDepth());
danstrider 11:3b241ecb75ed 507 pc().printf("pitch: %3.1f\r\n",imu().getPitch());
danstrider 11:3b241ecb75ed 508 pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
danstrider 11:3b241ecb75ed 509 pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
danstrider 11:3b241ecb75ed 510 pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
danstrider 11:3b241ecb75ed 511 pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
danstrider 11:3b241ecb75ed 512 pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
danstrider 11:3b241ecb75ed 513 pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 514 }
danstrider 11:3b241ecb75ed 515 else {
danstrider 11:3b241ecb75ed 516 return (-1);
danstrider 11:3b241ecb75ed 517 }
mkelly10 5:15bd96205bb2 518 }
danstrider 11:3b241ecb75ed 519 return (-1);
mkelly10 5:15bd96205bb2 520 }
mkelly10 5:15bd96205bb2 521
danstrider 11:3b241ecb75ed 522 void stateMachine() {
danstrider 11:3b241ecb75ed 523 static int state = SIT_IDLE; // select starting state here
danstrider 11:3b241ecb75ed 524 static bool isTimeoutRunning = false; // default timer to not running
danstrider 11:3b241ecb75ed 525
danstrider 11:3b241ecb75ed 526 // finite state machine ... each state has at least one exit criteria
danstrider 11:3b241ecb75ed 527 switch (state) {
danstrider 11:3b241ecb75ed 528 case SIT_IDLE :
danstrider 11:3b241ecb75ed 529 // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
danstrider 11:3b241ecb75ed 530 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 531 showMenu();
danstrider 11:3b241ecb75ed 532 pc().printf("\r\nstate: SIT_IDLE\r\n");
danstrider 11:3b241ecb75ed 533 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 534
danstrider 11:3b241ecb75ed 535 // what is active?
danstrider 11:3b241ecb75ed 536 bce().stop();
danstrider 11:3b241ecb75ed 537 batt().stop();
danstrider 11:3b241ecb75ed 538 depthLoop().stop();
danstrider 11:3b241ecb75ed 539 pitchLoop().stop();
danstrider 11:3b241ecb75ed 540 }
danstrider 11:3b241ecb75ed 541 // how exit?
danstrider 11:3b241ecb75ed 542 if (pc().readable()) {
danstrider 11:3b241ecb75ed 543 state = KEYBOARD;
danstrider 11:3b241ecb75ed 544 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 545 }
danstrider 11:3b241ecb75ed 546 break;
danstrider 11:3b241ecb75ed 547
danstrider 11:3b241ecb75ed 548 case KEYBOARD :
danstrider 11:3b241ecb75ed 549 pc().printf("\r\nstate: KEYBOARD\r\n");
danstrider 11:3b241ecb75ed 550 if (pc().readable()) {
danstrider 11:3b241ecb75ed 551 state = keyboard(); // get new state command
danstrider 11:3b241ecb75ed 552 if (state == -1) { // error, that wasn't a new state command
danstrider 11:3b241ecb75ed 553 state = SIT_IDLE;
danstrider 11:3b241ecb75ed 554 }
danstrider 11:3b241ecb75ed 555 //pc().printf("new state is: %d \r\n",state);
danstrider 11:3b241ecb75ed 556 }
danstrider 11:3b241ecb75ed 557 break;
danstrider 10:085ab7328054 558
danstrider 11:3b241ecb75ed 559 case EMERGENCY_CLIMB :
danstrider 11:3b241ecb75ed 560 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 561 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 562 pc().printf("\r\nstate: EMERGENCY_CLIMB\r\n");
danstrider 11:3b241ecb75ed 563 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 564 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 565 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 566
danstrider 11:3b241ecb75ed 567 // what needs to be started?
danstrider 11:3b241ecb75ed 568 bce().start();
danstrider 11:3b241ecb75ed 569 batt().start();
danstrider 11:3b241ecb75ed 570
danstrider 11:3b241ecb75ed 571 // what is active?
danstrider 11:3b241ecb75ed 572 bce().setPosition_mm(bce().getTravelLimit());
danstrider 11:3b241ecb75ed 573 batt().setPosition_mm(0.0);
danstrider 11:3b241ecb75ed 574 }
danstrider 11:3b241ecb75ed 575 // how exit?
danstrider 11:3b241ecb75ed 576 if (timer > timeout) {
danstrider 11:3b241ecb75ed 577 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 578 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 579 timer.reset();
danstrider 11:3b241ecb75ed 580 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 581 }
danstrider 11:3b241ecb75ed 582 else if (depth().getDepth() < 0.2) {
danstrider 11:3b241ecb75ed 583 pc().printf("depth: %3.1f, cmd: 0.5\r\n",depth().getDepth());
danstrider 11:3b241ecb75ed 584 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 585 timer.reset();
danstrider 11:3b241ecb75ed 586 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 587 }
danstrider 11:3b241ecb75ed 588 break;
danstrider 11:3b241ecb75ed 589
danstrider 11:3b241ecb75ed 590 case FIND_NEUTRAL :
danstrider 11:3b241ecb75ed 591 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 592 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 593 pc().printf("\r\nstate: FIND_NEUTRAL\r\n");
danstrider 11:3b241ecb75ed 594 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 595 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 596 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 597
danstrider 11:3b241ecb75ed 598 // what needs to be started?
danstrider 11:3b241ecb75ed 599 bce().start();
danstrider 11:3b241ecb75ed 600 batt().start();
danstrider 11:3b241ecb75ed 601 depthLoop().start();
danstrider 11:3b241ecb75ed 602 pitchLoop().start();
danstrider 11:3b241ecb75ed 603
danstrider 11:3b241ecb75ed 604 // what is active?
danstrider 11:3b241ecb75ed 605 depthLoop().setCommand(depthCommand);
danstrider 11:3b241ecb75ed 606 pitchLoop().setCommand(pitchCommand);
danstrider 11:3b241ecb75ed 607 }
danstrider 11:3b241ecb75ed 608 // how exit?
danstrider 11:3b241ecb75ed 609 if (timer > timeout) {
danstrider 11:3b241ecb75ed 610 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 611 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 612 timer.reset();
danstrider 11:3b241ecb75ed 613 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 614 }
danstrider 11:3b241ecb75ed 615 else if ((abs(depth().getDepth() - depthLoop().getCommand()) < depthTolerance) and
danstrider 11:3b241ecb75ed 616 (abs(imu().getPitch() - pitchLoop().getCommand()) < pitchTolerance)) {
danstrider 11:3b241ecb75ed 617 state = RISE;
danstrider 11:3b241ecb75ed 618 timer.reset();
danstrider 11:3b241ecb75ed 619 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 620 }
danstrider 11:3b241ecb75ed 621 // what is active?
danstrider 11:3b241ecb75ed 622 pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm \r", bce().getPosition_mm(), batt().getPosition_mm());
danstrider 11:3b241ecb75ed 623 bce().setPosition_mm(depthLoop().getOutput());
danstrider 11:3b241ecb75ed 624 batt().setPosition_mm(pitchLoop().getOutput());
danstrider 11:3b241ecb75ed 625 break;
mkelly10 5:15bd96205bb2 626
danstrider 11:3b241ecb75ed 627 case DIVE :
danstrider 11:3b241ecb75ed 628 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 629 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 630 pc().printf("\r\nstate: DIVE\r\n");
danstrider 11:3b241ecb75ed 631 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 632 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 633 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 634
danstrider 11:3b241ecb75ed 635 // what needs to be started?
danstrider 11:3b241ecb75ed 636 bce().start();
danstrider 11:3b241ecb75ed 637 batt().start();
danstrider 11:3b241ecb75ed 638 depthLoop().start();
danstrider 11:3b241ecb75ed 639 pitchLoop().start();
danstrider 11:3b241ecb75ed 640
danstrider 11:3b241ecb75ed 641 // what are the commands?
danstrider 11:3b241ecb75ed 642 depthLoop().setCommand(depthCommand);
danstrider 11:3b241ecb75ed 643 pitchLoop().setCommand(pitchCommand);
danstrider 11:3b241ecb75ed 644 pc().printf("depth cmd: %3.1f\r\n",depthLoop().getCommand());
danstrider 11:3b241ecb75ed 645 pc().printf("pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 646 }
danstrider 11:3b241ecb75ed 647 // how exit?
danstrider 11:3b241ecb75ed 648 if (timer > timeout) {
danstrider 11:3b241ecb75ed 649 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 650 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 651 timer.reset();
danstrider 11:3b241ecb75ed 652 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 653 }
danstrider 11:3b241ecb75ed 654 else if (depth().getDepth() > depthLoop().getCommand()) {
danstrider 11:3b241ecb75ed 655 pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depth().getDepth(), depthLoop().getCommand());
danstrider 11:3b241ecb75ed 656 state = RISE;
danstrider 11:3b241ecb75ed 657 timer.reset();
danstrider 11:3b241ecb75ed 658 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 659 }
danstrider 11:3b241ecb75ed 660 // what is active?
danstrider 11:3b241ecb75ed 661 pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm \r", bce().getPosition_mm(), batt().getPosition_mm());
danstrider 11:3b241ecb75ed 662 bce().setPosition_mm(depthLoop().getOutput());
danstrider 11:3b241ecb75ed 663 batt().setPosition_mm(pitchLoop().getOutput());
danstrider 11:3b241ecb75ed 664 break;
danstrider 11:3b241ecb75ed 665
danstrider 11:3b241ecb75ed 666 case RISE :
danstrider 11:3b241ecb75ed 667 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 668 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 669 pc().printf("\r\nstate: RISE\r\n");
danstrider 11:3b241ecb75ed 670 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 671 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 672 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 673
danstrider 11:3b241ecb75ed 674 // what needs to be started?
danstrider 11:3b241ecb75ed 675 bce().start();
danstrider 11:3b241ecb75ed 676 batt().start();
danstrider 11:3b241ecb75ed 677 depthLoop().start();
danstrider 11:3b241ecb75ed 678 pitchLoop().start();
danstrider 11:3b241ecb75ed 679
danstrider 11:3b241ecb75ed 680 // what are the commands?
danstrider 11:3b241ecb75ed 681 depthLoop().setCommand(0.0);
danstrider 11:3b241ecb75ed 682 pitchLoop().setCommand(-pitchCommand);
danstrider 11:3b241ecb75ed 683 pc().printf("depth cmd: 0.0\r\n");
danstrider 11:3b241ecb75ed 684 pc().printf("pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 685 }
danstrider 11:3b241ecb75ed 686 // how exit?
danstrider 11:3b241ecb75ed 687 if (timer > timeout) {
danstrider 11:3b241ecb75ed 688 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 689 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 690 timer.reset();
danstrider 11:3b241ecb75ed 691 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 692 }
danstrider 11:3b241ecb75ed 693 else if (depth().getDepth() < depthLoop().getCommand()) {
danstrider 11:3b241ecb75ed 694 pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depth().getDepth(), depthLoop().getCommand());
danstrider 11:3b241ecb75ed 695 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 696 timer.reset();
danstrider 11:3b241ecb75ed 697 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 698 }
danstrider 11:3b241ecb75ed 699 pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm \r", bce().getPosition_mm(), batt().getPosition_mm());
danstrider 11:3b241ecb75ed 700 // what is active?
danstrider 11:3b241ecb75ed 701 bce().setPosition_mm(depthLoop().getOutput());
danstrider 11:3b241ecb75ed 702 batt().setPosition_mm(pitchLoop().getOutput());
danstrider 11:3b241ecb75ed 703 break;
danstrider 11:3b241ecb75ed 704
danstrider 11:3b241ecb75ed 705 case FLOAT_LEVEL :
danstrider 11:3b241ecb75ed 706 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 707 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 708 pc().printf("\r\nstate: FLOAT_LEVEL\r\n");
danstrider 11:3b241ecb75ed 709 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 710 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 711 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 712
danstrider 11:3b241ecb75ed 713 // what needs to be started?
danstrider 11:3b241ecb75ed 714 bce().start();
danstrider 11:3b241ecb75ed 715 batt().start();
danstrider 11:3b241ecb75ed 716 pitchLoop().start();
danstrider 11:3b241ecb75ed 717
danstrider 11:3b241ecb75ed 718 // what are the commands
danstrider 11:3b241ecb75ed 719 bce().setPosition_mm(bceFloatPosition);
danstrider 11:3b241ecb75ed 720 pitchLoop().setCommand(0.0);
danstrider 11:3b241ecb75ed 721 }
danstrider 11:3b241ecb75ed 722 // how exit?
danstrider 11:3b241ecb75ed 723 if (timer > timeout) {
danstrider 11:3b241ecb75ed 724 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 725 state = FLOAT_BROADCAST;
danstrider 11:3b241ecb75ed 726 timer.reset();
danstrider 11:3b241ecb75ed 727 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 728 }
danstrider 11:3b241ecb75ed 729 else if (abs(imu().getPitch() - pitchLoop().getCommand()) < abs(pitchTolerance)) {
danstrider 11:3b241ecb75ed 730 pc().printf("pitch: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",imu().getPitch(), pitchLoop().getCommand(), pitchTolerance);
danstrider 11:3b241ecb75ed 731 state = FLOAT_BROADCAST;
danstrider 11:3b241ecb75ed 732 timer.reset();
danstrider 11:3b241ecb75ed 733 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 734 }
danstrider 11:3b241ecb75ed 735 // what is active?
danstrider 11:3b241ecb75ed 736 pc().printf("pitchLoop output: %3.1f, batt pos: %3.1f, piston pos: %3.1f\r", pitchLoop().getOutput(), batt().getPosition_mm(), bce().getPosition_mm());
danstrider 11:3b241ecb75ed 737 batt().setPosition_mm(pitchLoop().getOutput());
danstrider 11:3b241ecb75ed 738 break;
danstrider 11:3b241ecb75ed 739
danstrider 11:3b241ecb75ed 740 case FLOAT_BROADCAST :
danstrider 11:3b241ecb75ed 741 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 742 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 743 pc().printf("\r\nstate: FLOAT_BROADCAST\r\n");
danstrider 11:3b241ecb75ed 744 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 745 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 746 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 747
danstrider 11:3b241ecb75ed 748 // what needs to be started?
danstrider 11:3b241ecb75ed 749 bce().start();
danstrider 11:3b241ecb75ed 750 batt().start();
danstrider 11:3b241ecb75ed 751
danstrider 11:3b241ecb75ed 752 // what are the commands?
danstrider 11:3b241ecb75ed 753 bce().setPosition_mm(bceFloatPosition);
danstrider 11:3b241ecb75ed 754 batt().setPosition_mm(battFloatPosition);
danstrider 11:3b241ecb75ed 755 }
danstrider 11:3b241ecb75ed 756 // how exit?
danstrider 11:3b241ecb75ed 757 if (timer > timeout) {
danstrider 11:3b241ecb75ed 758 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 759 state = SIT_IDLE;
danstrider 11:3b241ecb75ed 760 timer.reset();
danstrider 11:3b241ecb75ed 761 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 762 }
danstrider 11:3b241ecb75ed 763 if (abs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) {
danstrider 11:3b241ecb75ed 764 pc().printf("position: %3.1f mm, set pos: %3.1f mm, deadband: %3.1f mm\r\n",bce().getPosition_mm(), bce().getSetPosition_mm(), bce().getDeadband());
danstrider 11:3b241ecb75ed 765 state = SIT_IDLE;
danstrider 11:3b241ecb75ed 766 timer.reset();
danstrider 11:3b241ecb75ed 767 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 768 }
danstrider 11:3b241ecb75ed 769 pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm \r", bce().getPosition_mm(), batt().getPosition_mm());
danstrider 11:3b241ecb75ed 770 break;
danstrider 11:3b241ecb75ed 771
danstrider 11:3b241ecb75ed 772 default :
danstrider 11:3b241ecb75ed 773 state = SIT_IDLE;
danstrider 11:3b241ecb75ed 774 }
danstrider 10:085ab7328054 775 }
tzyoung 0:ea293bbf9717 776
danstrider 11:3b241ecb75ed 777
danstrider 10:085ab7328054 778 int main() {
danstrider 10:085ab7328054 779 setup();
danstrider 10:085ab7328054 780 while(1) {
danstrider 11:3b241ecb75ed 781 led1() = !led1(); // blink
danstrider 11:3b241ecb75ed 782 //pc().printf("roll: %3.1f, pitch %3.1f, yaw: %3.1f\r", imu().getRoll(), imu().getPitch(), imu().getHeading());
danstrider 11:3b241ecb75ed 783 stateMachine();
danstrider 11:3b241ecb75ed 784 wait(0.2); // must have SOME wait or the outer loops in DIVE and RISE will lock the mbed
danstrider 10:085ab7328054 785 }
danstrider 10:085ab7328054 786 }