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@13:84fcbe1dcd62, 2017-10-30 (annotated)

Committer:
tnhnrl
Date:
Mon Oct 30 21:09:19 2017 +0000
Revision:
13:84fcbe1dcd62
Parent:
12:a0519d11d2b6
Child:
14:85b64a4d08e8
Tested hardware for bce and made several pause fixes.  Homing works. Added offset to pitch and depth.txt files, and the control loop works.  PID gain for depth had to be negative.  Next is to replace the motor and test pitch loop.

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();
tnhnrl 13:84fcbe1dcd62 112 depthLoop().start();
danstrider 11:3b241ecb75ed 113 depthLoop().setCommand(-2.5);
mkelly10 5:15bd96205bb2 114
danstrider 10:085ab7328054 115 pitchLoop().init();
tnhnrl 13:84fcbe1dcd62 116 pitchLoop().start();
danstrider 10:085ab7328054 117 pitchLoop().setCommand(0.0);
danstrider 10:085ab7328054 118
danstrider 10:085ab7328054 119 // do not leave this in. Check that PID gains are loading
danstrider 11:3b241ecb75ed 120 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 121 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 122 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 123 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 124 pc().printf("\n\r");
danstrider 10:085ab7328054 125 }
mkelly10 5:15bd96205bb2 126
danstrider 11:3b241ecb75ed 127
danstrider 11:3b241ecb75ed 128
danstrider 11:3b241ecb75ed 129 void keyboard_menu_BCE_PID_settings() {
danstrider 11:3b241ecb75ed 130 char PID_key;
danstrider 11:3b241ecb75ed 131 float gain_step_size = 0.01; // modify this to change gain step size
danstrider 11:3b241ecb75ed 132 float KP = bce().getControllerP(); // load current value
danstrider 11:3b241ecb75ed 133 float KI = bce().getControllerI(); // load current global value
danstrider 11:3b241ecb75ed 134 float KD = bce().getControllerD(); // load current global value
danstrider 11:3b241ecb75ed 135
danstrider 11:3b241ecb75ed 136 // show the menu
danstrider 11:3b241ecb75ed 137 pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
danstrider 11:3b241ecb75ed 138 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
danstrider 11:3b241ecb75ed 139 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.)\n\n\n\r");
danstrider 11:3b241ecb75ed 140 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 141
danstrider 11:3b241ecb75ed 142 // handle the key presses
danstrider 11:3b241ecb75ed 143 while(1) {
danstrider 11:3b241ecb75ed 144 // get the user's keystroke from either of the two inputs
danstrider 11:3b241ecb75ed 145 if (pc().readable()) {
danstrider 11:3b241ecb75ed 146 PID_key = pc().getc();
danstrider 11:3b241ecb75ed 147 }
danstrider 11:3b241ecb75ed 148 else {
danstrider 11:3b241ecb75ed 149 continue; // didn't get a user input, so keep waiting for it
danstrider 11:3b241ecb75ed 150 }
danstrider 11:3b241ecb75ed 151
danstrider 11:3b241ecb75ed 152 // handle the user's key input
danstrider 11:3b241ecb75ed 153 if (PID_key == '-') {
danstrider 11:3b241ecb75ed 154 KP -= gain_step_size;
danstrider 11:3b241ecb75ed 155 pc().printf("P gain: %0.5f \r\n", KP);
danstrider 11:3b241ecb75ed 156 }
danstrider 11:3b241ecb75ed 157 else if (PID_key == '=') {
danstrider 11:3b241ecb75ed 158 KP += gain_step_size;
danstrider 11:3b241ecb75ed 159 pc().printf("P gain: %0.5f \r\n", KP);
danstrider 11:3b241ecb75ed 160 }
danstrider 11:3b241ecb75ed 161 else if (PID_key == '[') {
danstrider 11:3b241ecb75ed 162 KI -= gain_step_size;
danstrider 11:3b241ecb75ed 163 pc().printf("I gain: %0.5f \r\n", KI);
danstrider 11:3b241ecb75ed 164 }
danstrider 11:3b241ecb75ed 165 else if (PID_key == ']') {
danstrider 11:3b241ecb75ed 166 KI += gain_step_size;
danstrider 11:3b241ecb75ed 167 pc().printf("I gain: %0.5f \r\n", KI);
danstrider 11:3b241ecb75ed 168 }
danstrider 11:3b241ecb75ed 169 else if (PID_key == ';') {
danstrider 11:3b241ecb75ed 170 KD -= gain_step_size;
danstrider 11:3b241ecb75ed 171 pc().printf("D gain: %0.5f \r\n", KD);
danstrider 11:3b241ecb75ed 172 }
danstrider 11:3b241ecb75ed 173 else if (PID_key == '\'') {
danstrider 11:3b241ecb75ed 174 KD += gain_step_size;
danstrider 11:3b241ecb75ed 175 pc().printf("D gain: %0.5f \r\n", KD);
danstrider 11:3b241ecb75ed 176 }
danstrider 11:3b241ecb75ed 177 else if (PID_key == 'S') { // user wants to save these modified values
danstrider 11:3b241ecb75ed 178 // set values
danstrider 11:3b241ecb75ed 179 bce().setControllerP(KP);
danstrider 11:3b241ecb75ed 180 bce().setControllerI(KI);
danstrider 11:3b241ecb75ed 181 bce().setControllerD(KD);
danstrider 11:3b241ecb75ed 182
danstrider 11:3b241ecb75ed 183 // save into "PID.cfg"
danstrider 11:3b241ecb75ed 184 //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 185 break; //exit the while loop
danstrider 11:3b241ecb75ed 186 }
danstrider 11:3b241ecb75ed 187 else if (PID_key == 'X') {
danstrider 11:3b241ecb75ed 188 break; //exit the while loop
danstrider 11:3b241ecb75ed 189 }
danstrider 11:3b241ecb75ed 190 else {
danstrider 11:3b241ecb75ed 191 pc().printf("\n\rThis key does nothing here. ");
danstrider 11:3b241ecb75ed 192 }
danstrider 11:3b241ecb75ed 193 }
danstrider 11:3b241ecb75ed 194 }
danstrider 11:3b241ecb75ed 195
danstrider 11:3b241ecb75ed 196 void keyboard_menu_DEPTH_PID_settings() {
danstrider 11:3b241ecb75ed 197 char PID_key;
danstrider 11:3b241ecb75ed 198 float gain_step_size = 0.01; // modify this to change gain step size
danstrider 11:3b241ecb75ed 199 float KP = depthLoop().getControllerP(); // load current global value
danstrider 11:3b241ecb75ed 200 float KI = depthLoop().getControllerI(); // load current global value
danstrider 11:3b241ecb75ed 201 float KD = depthLoop().getControllerD(); // load current global value
danstrider 11:3b241ecb75ed 202
danstrider 11:3b241ecb75ed 203 // show the menu
danstrider 11:3b241ecb75ed 204 pc().printf("\n\r1: Buoyancy Engine PID gain settings (MENU)");
danstrider 11:3b241ecb75ed 205 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
danstrider 11:3b241ecb75ed 206 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\n\n\r");
danstrider 11:3b241ecb75ed 207 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 208
danstrider 11:3b241ecb75ed 209 // handle the key presses
danstrider 11:3b241ecb75ed 210 while(1) {
danstrider 11:3b241ecb75ed 211 // get the user's keystroke from either of the two inputs
danstrider 11:3b241ecb75ed 212 if (pc().readable()) {
danstrider 11:3b241ecb75ed 213 PID_key = pc().getc();
danstrider 11:3b241ecb75ed 214 }
danstrider 11:3b241ecb75ed 215 else {
danstrider 11:3b241ecb75ed 216 continue; // didn't get a user input, so keep waiting for it
danstrider 11:3b241ecb75ed 217 }
danstrider 11:3b241ecb75ed 218
danstrider 11:3b241ecb75ed 219 // handle the user's key input
danstrider 11:3b241ecb75ed 220 if (PID_key == '-') {
danstrider 11:3b241ecb75ed 221 KP -= gain_step_size;
danstrider 11:3b241ecb75ed 222 pc().printf("P gain: %0.5f \r\n", KP);
danstrider 11:3b241ecb75ed 223 }
danstrider 11:3b241ecb75ed 224 else if (PID_key == '=') {
danstrider 11:3b241ecb75ed 225 KP += gain_step_size;
danstrider 11:3b241ecb75ed 226 pc().printf("P gain: %0.5f \r\n", KP);
danstrider 11:3b241ecb75ed 227 }
danstrider 11:3b241ecb75ed 228 else if (PID_key == '[') {
danstrider 11:3b241ecb75ed 229 KI -= gain_step_size;
danstrider 11:3b241ecb75ed 230 pc().printf("I gain: %0.5f \r\n", KI);
danstrider 11:3b241ecb75ed 231 }
danstrider 11:3b241ecb75ed 232 else if (PID_key == ']') {
danstrider 11:3b241ecb75ed 233 KI += gain_step_size;
danstrider 11:3b241ecb75ed 234 pc().printf("I gain: %0.5f \r\n", KI);
danstrider 11:3b241ecb75ed 235 }
danstrider 11:3b241ecb75ed 236 else if (PID_key == ';') {
danstrider 11:3b241ecb75ed 237 KD -= gain_step_size;
danstrider 11:3b241ecb75ed 238 pc().printf("D gain: %0.5f \r\n", KD);
danstrider 11:3b241ecb75ed 239 }
danstrider 11:3b241ecb75ed 240 else if (PID_key == '\'') {
danstrider 11:3b241ecb75ed 241 KD += gain_step_size;
danstrider 11:3b241ecb75ed 242 pc().printf("D gain: %0.5f \r\n", KD);
danstrider 11:3b241ecb75ed 243 }
danstrider 11:3b241ecb75ed 244 else if (PID_key == 'S') { // user wants to save these settings
danstrider 11:3b241ecb75ed 245 // set global values
danstrider 11:3b241ecb75ed 246 depthLoop().setControllerP(KP);
danstrider 11:3b241ecb75ed 247 depthLoop().setControllerI(KI);
danstrider 11:3b241ecb75ed 248 depthLoop().setControllerD(KD);
danstrider 11:3b241ecb75ed 249
danstrider 11:3b241ecb75ed 250 // save into "PID.cfg"
danstrider 11:3b241ecb75ed 251 //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 252 break; //exit the while loop
danstrider 11:3b241ecb75ed 253 }
danstrider 11:3b241ecb75ed 254 else if (PID_key == 'X') {
danstrider 11:3b241ecb75ed 255 break; //exit the while loop
danstrider 11:3b241ecb75ed 256 }
danstrider 11:3b241ecb75ed 257 else {
danstrider 11:3b241ecb75ed 258 pc().printf("\n\rThis key does nothing here. ");
danstrider 11:3b241ecb75ed 259 }
danstrider 11:3b241ecb75ed 260 }
danstrider 11:3b241ecb75ed 261 }
danstrider 11:3b241ecb75ed 262
danstrider 11:3b241ecb75ed 263 void keyboard_menu_BATT_PID_settings() {
danstrider 11:3b241ecb75ed 264 char PID_key;
danstrider 11:3b241ecb75ed 265 float gain_step_size = 0.01; // modify this to change gain step size
danstrider 11:3b241ecb75ed 266 float KP = batt().getControllerP(); // load current global value
danstrider 11:3b241ecb75ed 267 float KI = batt().getControllerI(); // load current global value
danstrider 11:3b241ecb75ed 268 float KD = batt().getControllerD(); // load current global value
danstrider 11:3b241ecb75ed 269
danstrider 11:3b241ecb75ed 270 // print the menu
danstrider 11:3b241ecb75ed 271 pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
danstrider 11:3b241ecb75ed 272 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
danstrider 11:3b241ecb75ed 273 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
danstrider 11:3b241ecb75ed 274 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 275
danstrider 11:3b241ecb75ed 276 // handle the key presses
danstrider 11:3b241ecb75ed 277 while(1) {
danstrider 11:3b241ecb75ed 278 // get the user's keystroke from either of the two inputs
danstrider 11:3b241ecb75ed 279 if (pc().readable()) {
danstrider 11:3b241ecb75ed 280 PID_key = pc().getc();
danstrider 11:3b241ecb75ed 281 }
danstrider 11:3b241ecb75ed 282 else {
danstrider 11:3b241ecb75ed 283 continue; // didn't get a user input, so keep waiting for it
danstrider 11:3b241ecb75ed 284 }
danstrider 11:3b241ecb75ed 285
danstrider 11:3b241ecb75ed 286 // handle the user's key input
danstrider 11:3b241ecb75ed 287 if (PID_key == '-') {
danstrider 11:3b241ecb75ed 288 KP -= gain_step_size;
danstrider 11:3b241ecb75ed 289 pc().printf("\rP gain: %0.5f ", KP);
danstrider 11:3b241ecb75ed 290 }
danstrider 11:3b241ecb75ed 291 else if (PID_key == '=') {
danstrider 11:3b241ecb75ed 292 KP += gain_step_size;
danstrider 11:3b241ecb75ed 293 pc().printf("\rP gain: %0.5f ", KP);
danstrider 11:3b241ecb75ed 294 }
danstrider 11:3b241ecb75ed 295 else if (PID_key == '[') {
danstrider 11:3b241ecb75ed 296 KI -= gain_step_size;
danstrider 11:3b241ecb75ed 297 pc().printf("\rI gain: %0.5f ", KI);
danstrider 11:3b241ecb75ed 298 }
danstrider 11:3b241ecb75ed 299 else if (PID_key == ']') {
danstrider 11:3b241ecb75ed 300 KI += gain_step_size;
danstrider 11:3b241ecb75ed 301 pc().printf("\rI gain: %0.5f ", KI);
danstrider 11:3b241ecb75ed 302 }
danstrider 11:3b241ecb75ed 303 else if (PID_key == ';') {
danstrider 11:3b241ecb75ed 304 KD -= gain_step_size;
danstrider 11:3b241ecb75ed 305 pc().printf("\rD gain: %0.5f ", KD);
danstrider 11:3b241ecb75ed 306 }
danstrider 11:3b241ecb75ed 307 else if (PID_key == '\'') {
danstrider 11:3b241ecb75ed 308 KD += gain_step_size;
danstrider 11:3b241ecb75ed 309 pc().printf("\rD gain: %0.5f ", KD);
danstrider 11:3b241ecb75ed 310 }
danstrider 11:3b241ecb75ed 311 else if (PID_key == 'S') { // user wants to save the modified values
danstrider 11:3b241ecb75ed 312 // set global values
danstrider 11:3b241ecb75ed 313 batt().setControllerP(KP);
danstrider 11:3b241ecb75ed 314 batt().setControllerI(KI);
danstrider 11:3b241ecb75ed 315 batt().setControllerD(KD);
danstrider 11:3b241ecb75ed 316
danstrider 11:3b241ecb75ed 317 // save to "PID.cfg" file
danstrider 11:3b241ecb75ed 318 //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 319 break; //exit the while loop
danstrider 11:3b241ecb75ed 320 }
danstrider 11:3b241ecb75ed 321 else if (PID_key == 'X') {
danstrider 11:3b241ecb75ed 322 break; //exit the while loop
danstrider 11:3b241ecb75ed 323 }
danstrider 11:3b241ecb75ed 324 else {
danstrider 11:3b241ecb75ed 325 pc().printf("This key does nothing here.\r");
danstrider 11:3b241ecb75ed 326 }
danstrider 11:3b241ecb75ed 327 }
danstrider 11:3b241ecb75ed 328 }
danstrider 11:3b241ecb75ed 329
danstrider 11:3b241ecb75ed 330 void keyboard_menu_PITCH_PID_settings() {
danstrider 11:3b241ecb75ed 331 char PID_key;
danstrider 11:3b241ecb75ed 332 float gain_step_size = 0.01; // modify this to change gain step size
danstrider 11:3b241ecb75ed 333 float KP = pitchLoop().getControllerP(); // load current global value
danstrider 11:3b241ecb75ed 334 float KI = pitchLoop().getControllerI(); // load current global value
danstrider 11:3b241ecb75ed 335 float KD = pitchLoop().getControllerD(); // load current global value
danstrider 11:3b241ecb75ed 336
danstrider 11:3b241ecb75ed 337 // print the menu
danstrider 11:3b241ecb75ed 338 pc().printf("\n\r2: Battery Motor PID gain settings (MENU)");
danstrider 11:3b241ecb75ed 339 pc().printf("\n\r(Adjust PID settings with the following keys: -= and [] and ;'");
danstrider 11:3b241ecb75ed 340 pc().printf("\n\r(Hit shift + X to exit w/o saving. Hit shift + S to save.\n\r");
danstrider 11:3b241ecb75ed 341 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 342
danstrider 11:3b241ecb75ed 343 // handle the key presses
danstrider 11:3b241ecb75ed 344 while(1) {
danstrider 11:3b241ecb75ed 345 // get the user's keystroke from either of the two inputs
danstrider 11:3b241ecb75ed 346 if (pc().readable()) {
danstrider 11:3b241ecb75ed 347 PID_key = pc().getc();
danstrider 11:3b241ecb75ed 348 }
danstrider 11:3b241ecb75ed 349 else {
danstrider 11:3b241ecb75ed 350 continue; // didn't get a user input, so keep waiting for it
danstrider 11:3b241ecb75ed 351 }
danstrider 11:3b241ecb75ed 352
danstrider 11:3b241ecb75ed 353 // handle the user's key input
danstrider 11:3b241ecb75ed 354 if (PID_key == '-') {
danstrider 11:3b241ecb75ed 355 KP -= gain_step_size;
danstrider 11:3b241ecb75ed 356 pc().printf("\rP gain: %0.5f ", KP);
danstrider 11:3b241ecb75ed 357 }
danstrider 11:3b241ecb75ed 358 else if (PID_key == '=') {
danstrider 11:3b241ecb75ed 359 KP += gain_step_size;
danstrider 11:3b241ecb75ed 360 pc().printf("\rP gain: %0.5f ", KP);
danstrider 11:3b241ecb75ed 361 }
danstrider 11:3b241ecb75ed 362 else if (PID_key == '[') {
danstrider 11:3b241ecb75ed 363 KI -= gain_step_size;
danstrider 11:3b241ecb75ed 364 pc().printf("\rI gain: %0.5f ", KI);
danstrider 11:3b241ecb75ed 365 }
danstrider 11:3b241ecb75ed 366 else if (PID_key == ']') {
danstrider 11:3b241ecb75ed 367 KI += gain_step_size;
danstrider 11:3b241ecb75ed 368 pc().printf("\rI gain: %0.5f ", KI);
danstrider 11:3b241ecb75ed 369 }
danstrider 11:3b241ecb75ed 370 else if (PID_key == ';') {
danstrider 11:3b241ecb75ed 371 KD -= gain_step_size;
danstrider 11:3b241ecb75ed 372 pc().printf("\rD gain: %0.5f ", KD);
danstrider 11:3b241ecb75ed 373 }
danstrider 11:3b241ecb75ed 374 else if (PID_key == '\'') {
danstrider 11:3b241ecb75ed 375 KD += gain_step_size;
danstrider 11:3b241ecb75ed 376 pc().printf("\rD gain: %0.5f ", KD);
danstrider 11:3b241ecb75ed 377 }
danstrider 11:3b241ecb75ed 378 else if (PID_key == 'S') { // user wants to save the modified values
danstrider 11:3b241ecb75ed 379 // set global values
danstrider 11:3b241ecb75ed 380 pitchLoop().setControllerP(KP);
danstrider 11:3b241ecb75ed 381 pitchLoop().setControllerI(KI);
danstrider 11:3b241ecb75ed 382 pitchLoop().setControllerD(KD);
danstrider 11:3b241ecb75ed 383
danstrider 11:3b241ecb75ed 384 //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 385 break; //exit the while loop
danstrider 11:3b241ecb75ed 386 }
danstrider 11:3b241ecb75ed 387 else if (PID_key == 'X') {
danstrider 11:3b241ecb75ed 388 break; //exit the while loop
danstrider 11:3b241ecb75ed 389 }
danstrider 11:3b241ecb75ed 390 else {
danstrider 11:3b241ecb75ed 391 pc().printf("This key does nothing here.\r");
danstrider 11:3b241ecb75ed 392 }
danstrider 11:3b241ecb75ed 393 }
danstrider 11:3b241ecb75ed 394 }
danstrider 11:3b241ecb75ed 395
danstrider 11:3b241ecb75ed 396
danstrider 11:3b241ecb75ed 397 // output the keyboard menu for user's reference
danstrider 11:3b241ecb75ed 398 void showMenu() {
danstrider 11:3b241ecb75ed 399 pc().printf("\r\r\n\nKEYBOARD MENU:\r\r\n");
danstrider 11:3b241ecb75ed 400 pc().printf(" N to find neutral\r\n");
danstrider 11:3b241ecb75ed 401 pc().printf(" D to initiate dive cycle\r\n");
danstrider 11:3b241ecb75ed 402 pc().printf(" R to initiate rise\r\n");
danstrider 11:3b241ecb75ed 403 pc().printf(" L to float level\r\n");
danstrider 11:3b241ecb75ed 404 pc().printf(" B to float at broadcast pitch\r\n");
danstrider 11:3b241ecb75ed 405 pc().printf(" E to initiate emergency climb\r\n");
mkelly10 12:a0519d11d2b6 406 pc().printf(" H to run homing sequence on both BCE and Batt\r\n");
danstrider 11:3b241ecb75ed 407 pc().printf("Q/W to decrease/increase pitch setpoint: %3.1f\r\n",pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 408 pc().printf("A/S to decrease/increase depth setpoint: %3.1f\r\n",depthLoop().getCommand());
danstrider 11:3b241ecb75ed 409 pc().printf("+/- to decrease/increase timeout: %d s\r\n",timeout);
danstrider 11:3b241ecb75ed 410 pc().printf(" 1 BCE PID sub-menu\r\n");
danstrider 11:3b241ecb75ed 411 pc().printf(" 2 BATT PID sub-menu\r\n");
danstrider 11:3b241ecb75ed 412 pc().printf(" 3 Depth PID sub-menu\r\n");
danstrider 11:3b241ecb75ed 413 pc().printf(" 4 Pitch PID sub-menu\r\n");
danstrider 11:3b241ecb75ed 414 pc().printf(" C See sensor readings\r\n");
danstrider 11:3b241ecb75ed 415 pc().printf(" ? to reset mbed\r\n");
danstrider 11:3b241ecb75ed 416 }
danstrider 11:3b241ecb75ed 417
danstrider 11:3b241ecb75ed 418 // keyboard currently handles a key at a time
danstrider 11:3b241ecb75ed 419 // returns -1 if not a state command
danstrider 11:3b241ecb75ed 420 // returns a positive number to command a new state
danstrider 11:3b241ecb75ed 421 int keyboard() {
danstrider 10:085ab7328054 422 char userInput;
danstrider 10:085ab7328054 423
danstrider 10:085ab7328054 424 // check keyboard and make settings changes as requested
danstrider 10:085ab7328054 425 if (pc().readable()) {
danstrider 10:085ab7328054 426 // get the key
danstrider 10:085ab7328054 427 userInput = pc().getc();
mkelly10 5:15bd96205bb2 428
danstrider 10:085ab7328054 429 // check command against desired control buttons
danstrider 11:3b241ecb75ed 430 // change state
danstrider 11:3b241ecb75ed 431 if (userInput == 'D' or userInput == 'd') {
danstrider 11:3b241ecb75ed 432 return DIVE;
danstrider 10:085ab7328054 433 }
danstrider 11:3b241ecb75ed 434 else if (userInput == 'N' or userInput == 'n') {
danstrider 11:3b241ecb75ed 435 return FIND_NEUTRAL;
danstrider 11:3b241ecb75ed 436 }
danstrider 11:3b241ecb75ed 437 else if (userInput == 'R' or userInput == 'r') {
danstrider 11:3b241ecb75ed 438 return RISE;
danstrider 11:3b241ecb75ed 439 }
danstrider 11:3b241ecb75ed 440 else if (userInput == 'L' or userInput == 'l') {
danstrider 11:3b241ecb75ed 441 return FLOAT_LEVEL;
danstrider 10:085ab7328054 442 }
danstrider 11:3b241ecb75ed 443 else if (userInput == 'B' or userInput == 'b') {
danstrider 11:3b241ecb75ed 444 return FLOAT_BROADCAST;
danstrider 11:3b241ecb75ed 445 }
danstrider 11:3b241ecb75ed 446 else if (userInput == 'E' or userInput == 'e') {
danstrider 11:3b241ecb75ed 447 return EMERGENCY_CLIMB;
mkelly10 5:15bd96205bb2 448 }
mkelly10 12:a0519d11d2b6 449 else if (userInput == 'H' or userInput == 'h') {
mkelly10 12:a0519d11d2b6 450 pc().printf("running homing procedure\r\n");
mkelly10 12:a0519d11d2b6 451 bce().unpause(); bce().homePiston(); bce().pause();
mkelly10 12:a0519d11d2b6 452 batt().unpause(); batt().homePiston(); batt().pause();
mkelly10 12:a0519d11d2b6 453 return SIT_IDLE;
mkelly10 12:a0519d11d2b6 454 }
danstrider 10:085ab7328054 455 else if (userInput == '?') {
danstrider 10:085ab7328054 456 pc().printf("\n\n\n>>> Resetting MBED <<<\n\n\n");
danstrider 11:3b241ecb75ed 457 wait(0.5);
danstrider 10:085ab7328054 458 mbed_reset();
danstrider 10:085ab7328054 459 }
danstrider 11:3b241ecb75ed 460
danstrider 11:3b241ecb75ed 461 // change settings
danstrider 11:3b241ecb75ed 462 else if (userInput == 'Q' or userInput == 'q') {
danstrider 11:3b241ecb75ed 463 pitchCommand -= 0.5; //decrement the pitch setpoint
danstrider 11:3b241ecb75ed 464 pitchLoop().setCommand(pitchCommand);
danstrider 11:3b241ecb75ed 465 pc().printf(">>> new pitch angle setpoint: %0.3f deg (decreased)\r\n", pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 466 }
danstrider 11:3b241ecb75ed 467 else if (userInput == 'W' or userInput == 'w') {
danstrider 11:3b241ecb75ed 468 pitchCommand += 0.5; //increment the pitch setpoint
danstrider 11:3b241ecb75ed 469 pitchLoop().setCommand(pitchCommand);
danstrider 11:3b241ecb75ed 470 pc().printf(">>> new pitch angle setpoint: %0.3f deg (increased)\r\n", pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 471 }
danstrider 11:3b241ecb75ed 472 else if (userInput == 'A' or userInput == 'a') {
danstrider 11:3b241ecb75ed 473 depthCommand -= 0.5; //decrement the depth setpoint
danstrider 11:3b241ecb75ed 474 depthLoop().setCommand(depthCommand);
danstrider 11:3b241ecb75ed 475 pc().printf(">>> new depth (ft) setpoint: %0.3f ft (sink)\r\n", depthLoop().getCommand());
danstrider 11:3b241ecb75ed 476 }
danstrider 11:3b241ecb75ed 477 else if (userInput == 'S' or userInput == 's') {
danstrider 11:3b241ecb75ed 478 depthCommand += 0.5; //increment the depth setpoint
danstrider 11:3b241ecb75ed 479 depthLoop().setCommand(depthCommand);
danstrider 11:3b241ecb75ed 480 pc().printf(">>> new depth setpoint: %0.3f ft (rise)\r\n", depthLoop().getCommand());
danstrider 11:3b241ecb75ed 481 }
danstrider 11:3b241ecb75ed 482 else if (userInput == '-') {
danstrider 11:3b241ecb75ed 483 timeout -= 10.0; //decrement the timeout
danstrider 11:3b241ecb75ed 484 pc().printf(">>> timeout decreased: %d\r\n", timeout);
danstrider 11:3b241ecb75ed 485 }
danstrider 11:3b241ecb75ed 486 else if (userInput == '=' or userInput == '+') {
danstrider 11:3b241ecb75ed 487 timeout += 10.0; //increment the timeout
danstrider 11:3b241ecb75ed 488 pc().printf(">>> timeout increased: %d\r\n", timeout);
danstrider 11:3b241ecb75ed 489 }
danstrider 11:3b241ecb75ed 490
danstrider 11:3b241ecb75ed 491 // add keyboard commands to move the neutral zero offsets, both bce and batt
danstrider 11:3b241ecb75ed 492
danstrider 11:3b241ecb75ed 493 // go to sub-menus for the PID gains (this is blocking)
danstrider 11:3b241ecb75ed 494 else if (userInput == '1') {
danstrider 11:3b241ecb75ed 495 keyboard_menu_BCE_PID_settings();
danstrider 11:3b241ecb75ed 496 }
danstrider 11:3b241ecb75ed 497 else if (userInput == '2') {
danstrider 11:3b241ecb75ed 498 keyboard_menu_BATT_PID_settings();
danstrider 11:3b241ecb75ed 499 }
danstrider 11:3b241ecb75ed 500 else if (userInput == '3') {
danstrider 11:3b241ecb75ed 501 keyboard_menu_DEPTH_PID_settings();
danstrider 11:3b241ecb75ed 502 }
danstrider 11:3b241ecb75ed 503 else if (userInput == '4') {
danstrider 11:3b241ecb75ed 504 keyboard_menu_PITCH_PID_settings();
danstrider 11:3b241ecb75ed 505 }
danstrider 11:3b241ecb75ed 506
danstrider 11:3b241ecb75ed 507 else if (userInput == 'C' or userInput == 'c') {
tnhnrl 13:84fcbe1dcd62 508 pc().printf("depth: %3.1f\r\n",depthLoop().getPosition());
danstrider 11:3b241ecb75ed 509 pc().printf("pitch: %3.1f\r\n",imu().getPitch());
danstrider 11:3b241ecb75ed 510 pc().printf("bce().getPosition_mm(): %3.1f\r\n",bce().getPosition_mm());
danstrider 11:3b241ecb75ed 511 pc().printf("bce().getSetPosition_mm(): %3.1f\r\n",bce().getSetPosition_mm());
danstrider 11:3b241ecb75ed 512 pc().printf("batt().getPosition_mm(): %3.1f\r\n",batt().getPosition_mm());
danstrider 11:3b241ecb75ed 513 pc().printf("batt().getSetPosition_mm(): %3.1f\r\n",batt().getSetPosition_mm());
danstrider 11:3b241ecb75ed 514 pc().printf("depthLoop().getCommand(): %3.1f\r\n",depthLoop().getCommand());
danstrider 11:3b241ecb75ed 515 pc().printf("pitchLoop().getCommand(): %3.1f\r\n",pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 516 }
danstrider 11:3b241ecb75ed 517 else {
danstrider 11:3b241ecb75ed 518 return (-1);
danstrider 11:3b241ecb75ed 519 }
mkelly10 5:15bd96205bb2 520 }
danstrider 11:3b241ecb75ed 521 return (-1);
mkelly10 5:15bd96205bb2 522 }
mkelly10 5:15bd96205bb2 523
danstrider 11:3b241ecb75ed 524 void stateMachine() {
danstrider 11:3b241ecb75ed 525 static int state = SIT_IDLE; // select starting state here
danstrider 11:3b241ecb75ed 526 static bool isTimeoutRunning = false; // default timer to not running
danstrider 11:3b241ecb75ed 527
danstrider 11:3b241ecb75ed 528 // finite state machine ... each state has at least one exit criteria
danstrider 11:3b241ecb75ed 529 switch (state) {
danstrider 11:3b241ecb75ed 530 case SIT_IDLE :
danstrider 11:3b241ecb75ed 531 // there actually is no timeout for SIT_IDLE, but this enables some one-shot actions
danstrider 11:3b241ecb75ed 532 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 533 showMenu();
danstrider 11:3b241ecb75ed 534 pc().printf("\r\nstate: SIT_IDLE\r\n");
danstrider 11:3b241ecb75ed 535 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 536
danstrider 11:3b241ecb75ed 537 // what is active?
mkelly10 12:a0519d11d2b6 538 bce().pause();
mkelly10 12:a0519d11d2b6 539 batt().pause();
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 }
tnhnrl 13:84fcbe1dcd62 582 else if (depthLoop().getPosition() < 0.2) {
tnhnrl 13:84fcbe1dcd62 583 pc().printf("depth: %3.1f, cmd: 0.5\r\n",depthLoop().getPosition());
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();
tnhnrl 13:84fcbe1dcd62 601
danstrider 11:3b241ecb75ed 602 // what is active?
danstrider 11:3b241ecb75ed 603 depthLoop().setCommand(depthCommand);
danstrider 11:3b241ecb75ed 604 pitchLoop().setCommand(pitchCommand);
danstrider 11:3b241ecb75ed 605 }
danstrider 11:3b241ecb75ed 606 // how exit?
danstrider 11:3b241ecb75ed 607 if (timer > timeout) {
danstrider 11:3b241ecb75ed 608 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 609 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 610 timer.reset();
danstrider 11:3b241ecb75ed 611 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 612 }
tnhnrl 13:84fcbe1dcd62 613 else if ((abs(depthLoop().getPosition() - depthLoop().getCommand()) < depthTolerance) and
danstrider 11:3b241ecb75ed 614 (abs(imu().getPitch() - pitchLoop().getCommand()) < pitchTolerance)) {
danstrider 11:3b241ecb75ed 615 state = RISE;
danstrider 11:3b241ecb75ed 616 timer.reset();
danstrider 11:3b241ecb75ed 617 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 618 }
danstrider 11:3b241ecb75ed 619 // what is active?
tnhnrl 13:84fcbe1dcd62 620 pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition());
danstrider 11:3b241ecb75ed 621 bce().setPosition_mm(depthLoop().getOutput());
danstrider 11:3b241ecb75ed 622 batt().setPosition_mm(pitchLoop().getOutput());
danstrider 11:3b241ecb75ed 623 break;
mkelly10 5:15bd96205bb2 624
danstrider 11:3b241ecb75ed 625 case DIVE :
danstrider 11:3b241ecb75ed 626 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 627 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 628 pc().printf("\r\nstate: DIVE\r\n");
danstrider 11:3b241ecb75ed 629 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 630 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 631 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 632
danstrider 11:3b241ecb75ed 633 // what needs to be started?
mkelly10 12:a0519d11d2b6 634 bce().unpause();
mkelly10 12:a0519d11d2b6 635 batt().unpause();
danstrider 11:3b241ecb75ed 636
danstrider 11:3b241ecb75ed 637 // what are the commands?
danstrider 11:3b241ecb75ed 638 depthLoop().setCommand(depthCommand);
danstrider 11:3b241ecb75ed 639 pitchLoop().setCommand(pitchCommand);
danstrider 11:3b241ecb75ed 640 pc().printf("depth cmd: %3.1f\r\n",depthLoop().getCommand());
danstrider 11:3b241ecb75ed 641 pc().printf("pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 642 }
danstrider 11:3b241ecb75ed 643 // how exit?
danstrider 11:3b241ecb75ed 644 if (timer > timeout) {
danstrider 11:3b241ecb75ed 645 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 646 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 647 timer.reset();
danstrider 11:3b241ecb75ed 648 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 649 }
tnhnrl 13:84fcbe1dcd62 650 else if (depthLoop().getPosition() > depthLoop().getCommand()) {
tnhnrl 13:84fcbe1dcd62 651 pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
danstrider 11:3b241ecb75ed 652 state = RISE;
danstrider 11:3b241ecb75ed 653 timer.reset();
danstrider 11:3b241ecb75ed 654 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 655 }
danstrider 11:3b241ecb75ed 656 // what is active?
tnhnrl 13:84fcbe1dcd62 657 pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition());
danstrider 11:3b241ecb75ed 658 bce().setPosition_mm(depthLoop().getOutput());
danstrider 11:3b241ecb75ed 659 batt().setPosition_mm(pitchLoop().getOutput());
danstrider 11:3b241ecb75ed 660 break;
danstrider 11:3b241ecb75ed 661
danstrider 11:3b241ecb75ed 662 case RISE :
danstrider 11:3b241ecb75ed 663 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 664 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 665 pc().printf("\r\nstate: RISE\r\n");
danstrider 11:3b241ecb75ed 666 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 667 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 668 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 669
danstrider 11:3b241ecb75ed 670 // what needs to be started?
mkelly10 12:a0519d11d2b6 671 bce().unpause();
mkelly10 12:a0519d11d2b6 672 batt().unpause();
tnhnrl 13:84fcbe1dcd62 673
danstrider 11:3b241ecb75ed 674 // what are the commands?
danstrider 11:3b241ecb75ed 675 depthLoop().setCommand(0.0);
danstrider 11:3b241ecb75ed 676 pitchLoop().setCommand(-pitchCommand);
danstrider 11:3b241ecb75ed 677 pc().printf("depth cmd: 0.0\r\n");
danstrider 11:3b241ecb75ed 678 pc().printf("pitch cmd: %3.1f\r\n",pitchLoop().getCommand());
danstrider 11:3b241ecb75ed 679 }
danstrider 11:3b241ecb75ed 680 // how exit?
danstrider 11:3b241ecb75ed 681 if (timer > timeout) {
danstrider 11:3b241ecb75ed 682 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 683 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 684 timer.reset();
danstrider 11:3b241ecb75ed 685 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 686 }
tnhnrl 13:84fcbe1dcd62 687 else if (depthLoop().getPosition() < depthLoop().getCommand()) {
tnhnrl 13:84fcbe1dcd62 688 pc().printf("depth: %3.1f, cmd: %3.1f\r\n", depthLoop().getPosition(), depthLoop().getCommand());
danstrider 11:3b241ecb75ed 689 state = FLOAT_LEVEL;
danstrider 11:3b241ecb75ed 690 timer.reset();
danstrider 11:3b241ecb75ed 691 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 692 }
tnhnrl 13:84fcbe1dcd62 693 pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition());
danstrider 11:3b241ecb75ed 694 // what is active?
danstrider 11:3b241ecb75ed 695 bce().setPosition_mm(depthLoop().getOutput());
danstrider 11:3b241ecb75ed 696 batt().setPosition_mm(pitchLoop().getOutput());
danstrider 11:3b241ecb75ed 697 break;
danstrider 11:3b241ecb75ed 698
danstrider 11:3b241ecb75ed 699 case FLOAT_LEVEL :
danstrider 11:3b241ecb75ed 700 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 701 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 702 pc().printf("\r\nstate: FLOAT_LEVEL\r\n");
danstrider 11:3b241ecb75ed 703 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 704 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 705 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 706
danstrider 11:3b241ecb75ed 707 // what needs to be started?
mkelly10 12:a0519d11d2b6 708 bce().unpause();
mkelly10 12:a0519d11d2b6 709 batt().unpause();
tnhnrl 13:84fcbe1dcd62 710
danstrider 11:3b241ecb75ed 711 // what are the commands
danstrider 11:3b241ecb75ed 712 bce().setPosition_mm(bceFloatPosition);
danstrider 11:3b241ecb75ed 713 pitchLoop().setCommand(0.0);
danstrider 11:3b241ecb75ed 714 }
danstrider 11:3b241ecb75ed 715 // how exit?
danstrider 11:3b241ecb75ed 716 if (timer > timeout) {
danstrider 11:3b241ecb75ed 717 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 718 state = FLOAT_BROADCAST;
danstrider 11:3b241ecb75ed 719 timer.reset();
danstrider 11:3b241ecb75ed 720 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 721 }
danstrider 11:3b241ecb75ed 722 else if (abs(imu().getPitch() - pitchLoop().getCommand()) < abs(pitchTolerance)) {
danstrider 11:3b241ecb75ed 723 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 724 state = FLOAT_BROADCAST;
danstrider 11:3b241ecb75ed 725 timer.reset();
danstrider 11:3b241ecb75ed 726 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 727 }
danstrider 11:3b241ecb75ed 728 // what is active?
danstrider 11:3b241ecb75ed 729 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 730 batt().setPosition_mm(pitchLoop().getOutput());
danstrider 11:3b241ecb75ed 731 break;
danstrider 11:3b241ecb75ed 732
danstrider 11:3b241ecb75ed 733 case FLOAT_BROADCAST :
danstrider 11:3b241ecb75ed 734 // start local state timer and init any other one-shot actions
danstrider 11:3b241ecb75ed 735 if (!isTimeoutRunning) {
danstrider 11:3b241ecb75ed 736 pc().printf("\r\nstate: FLOAT_BROADCAST\r\n");
danstrider 11:3b241ecb75ed 737 timer.reset(); // timer goes back to zero
danstrider 11:3b241ecb75ed 738 timer.start(); // background timer starts running
danstrider 11:3b241ecb75ed 739 isTimeoutRunning = true;
danstrider 11:3b241ecb75ed 740
danstrider 11:3b241ecb75ed 741 // what needs to be started?
mkelly10 12:a0519d11d2b6 742 bce().unpause();
mkelly10 12:a0519d11d2b6 743 batt().unpause();
danstrider 11:3b241ecb75ed 744
danstrider 11:3b241ecb75ed 745 // what are the commands?
danstrider 11:3b241ecb75ed 746 bce().setPosition_mm(bceFloatPosition);
danstrider 11:3b241ecb75ed 747 batt().setPosition_mm(battFloatPosition);
danstrider 11:3b241ecb75ed 748 }
danstrider 11:3b241ecb75ed 749 // how exit?
danstrider 11:3b241ecb75ed 750 if (timer > timeout) {
danstrider 11:3b241ecb75ed 751 pc().printf("timed out\r\n");
danstrider 11:3b241ecb75ed 752 state = SIT_IDLE;
danstrider 11:3b241ecb75ed 753 timer.reset();
danstrider 11:3b241ecb75ed 754 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 755 }
danstrider 11:3b241ecb75ed 756 if (abs(bce().getPosition_mm() - bce().getSetPosition_mm()) < bce().getDeadband()) {
danstrider 11:3b241ecb75ed 757 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 758 state = SIT_IDLE;
danstrider 11:3b241ecb75ed 759 timer.reset();
danstrider 11:3b241ecb75ed 760 isTimeoutRunning = false;
danstrider 11:3b241ecb75ed 761 }
tnhnrl 13:84fcbe1dcd62 762 pc().printf("bce pos: %3.1f mm, batt pos: %3.1f mm (depthLoop POS: %3.1f ft) \r", bce().getPosition_mm(), batt().getPosition_mm(), depthLoop().getPosition());
danstrider 11:3b241ecb75ed 763 break;
danstrider 11:3b241ecb75ed 764
danstrider 11:3b241ecb75ed 765 default :
danstrider 11:3b241ecb75ed 766 state = SIT_IDLE;
danstrider 11:3b241ecb75ed 767 }
danstrider 10:085ab7328054 768 }
tzyoung 0:ea293bbf9717 769
danstrider 11:3b241ecb75ed 770
danstrider 10:085ab7328054 771 int main() {
danstrider 10:085ab7328054 772 setup();
danstrider 10:085ab7328054 773 while(1) {
danstrider 11:3b241ecb75ed 774 led1() = !led1(); // blink
danstrider 11:3b241ecb75ed 775 //pc().printf("roll: %3.1f, pitch %3.1f, yaw: %3.1f\r", imu().getRoll(), imu().getPitch(), imu().getHeading());
danstrider 11:3b241ecb75ed 776 stateMachine();
danstrider 10:085ab7328054 777 }
danstrider 10:085ab7328054 778 }