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@12:a0519d11d2b6, 2017-10-30 (annotated)

Committer:
mkelly10
Date:
Mon Oct 30 19:46:38 2017 +0000
Revision:
12:a0519d11d2b6
Parent:
11:3b241ecb75ed
Child:
13:84fcbe1dcd62
Changed stops and starts to pause and unpause. Added _init true flag to stop function

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();
mkelly10 12:a0519d11d2b6 104 bce().pause(); // start by not moving
tzyoung 4:66f13fbb035d 105
danstrider 10:085ab7328054 106 batt().init();
tzyoung 2:892b58e56712 107 batt().start();
mkelly10 12:a0519d11d2b6 108 batt().pause(); // 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");
mkelly10 12:a0519d11d2b6 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 }
mkelly10 12:a0519d11d2b6 447 else if (userInput == 'H' or userInput == 'h') {
mkelly10 12:a0519d11d2b6 448 pc().printf("running homing procedure\r\n");
mkelly10 12:a0519d11d2b6 449 bce().unpause(); bce().homePiston(); bce().pause();
mkelly10 12:a0519d11d2b6 450 batt().unpause(); batt().homePiston(); batt().pause();
mkelly10 12:a0519d11d2b6 451 return SIT_IDLE;
mkelly10 12:a0519d11d2b6 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?
mkelly10 12:a0519d11d2b6 536 bce().pause();
mkelly10 12:a0519d11d2b6 537 batt().pause();
mkelly10 12:a0519d11d2b6 538 // depthLoop().stop();
mkelly10 12:a0519d11d2b6 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?
mkelly10 12:a0519d11d2b6 568 bce().unpause();
mkelly10 12:a0519d11d2b6 569 batt().unpause();
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?
mkelly10 12:a0519d11d2b6 599 bce().unpause();
mkelly10 12:a0519d11d2b6 600 batt().unpause();
mkelly10 12:a0519d11d2b6 601 // depthLoop().start();
mkelly10 12:a0519d11d2b6 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?
mkelly10 12:a0519d11d2b6 636 bce().unpause();
mkelly10 12:a0519d11d2b6 637 batt().unpause();
mkelly10 12:a0519d11d2b6 638 // depthLoop().start();
mkelly10 12:a0519d11d2b6 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?
mkelly10 12:a0519d11d2b6 675 bce().unpause();
mkelly10 12:a0519d11d2b6 676 batt().unpause();
mkelly10 12:a0519d11d2b6 677 // depthLoop().start();
mkelly10 12:a0519d11d2b6 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?
mkelly10 12:a0519d11d2b6 714 bce().unpause();
mkelly10 12:a0519d11d2b6 715 batt().unpause();
mkelly10 12:a0519d11d2b6 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?
mkelly10 12:a0519d11d2b6 749 bce().unpause();
mkelly10 12:a0519d11d2b6 750 batt().unpause();
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 }