FSG / Mbed 2 deprecated 7_20_17_FSG_

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BCEmotor Battery_Linear_Actuator_ Controller_ IMU_code_ LTC1298_7_14 MODSERIAL PosVelFilter_7_14 System_ mbed

Fork of 7_14_17_FSG_working by Troy Holley

main.cpp@5:7421776f6b08, 2017-07-17 (annotated)

Committer:
tnhnrl
Date:
Mon Jul 17 14:28:53 2017 +0000
Revision:
5:7421776f6b08
Parent:
4:3c22d85a94a8
Child:
6:ce2cf7f4d7d5
7 14 version

;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mdavis30 0:381a84fad08b 1 #include "mbed.h"
mdavis30 0:381a84fad08b 2 #include "MODSERIAL.h" //for IMU (and got rid of regular serial library) #include "IMU_code.h"
mdavis30 0:381a84fad08b 3 #include "StaticDefs.hpp"
mdavis30 0:381a84fad08b 4 #include "ltc1298.hpp"
mdavis30 0:381a84fad08b 5
mdavis30 0:381a84fad08b 6 #include <cstdlib>
mdavis30 0:381a84fad08b 7
mdavis30 0:381a84fad08b 8 #include <string>
mdavis30 0:381a84fad08b 9 using namespace std;
mdavis30 0:381a84fad08b 10
tnhnrl 5:7421776f6b08 11 bool debug_mode = false;
tnhnrl 5:7421776f6b08 12
mdavis30 0:381a84fad08b 13 #include "IMU_code.h" //IMU code
mdavis30 0:381a84fad08b 14
mdavis30 0:381a84fad08b 15 #include "Battery_Linear_Actuator.h" //Battery Linear Actuator code (TROY) (FIX INCLUSION ISSUES, ports)
mdavis30 0:381a84fad08b 16
mdavis30 0:381a84fad08b 17 Serial PC(USBTX,USBRX); //tx, rx
mdavis30 0:381a84fad08b 18
mdavis30 0:381a84fad08b 19 extern "C" void mbed_reset(); //utilized to reset the mbed through the serial terminal
mdavis30 0:381a84fad08b 20
mdavis30 0:381a84fad08b 21 char Key;
mdavis30 0:381a84fad08b 22
mdavis30 0:381a84fad08b 23 string IMU_STRING = "";
mdavis30 0:381a84fad08b 24
mdavis30 0:381a84fad08b 25
mdavis30 0:381a84fad08b 26 DigitalOut led1(LED1);
mdavis30 0:381a84fad08b 27 DigitalOut led2(LED2);
mdavis30 0:381a84fad08b 28 DigitalOut led3(LED3);
mdavis30 0:381a84fad08b 29 DigitalOut led4(LED4);
mdavis30 0:381a84fad08b 30
mdavis30 0:381a84fad08b 31 AnalogIn pressure_analog_in(A5); //Initialize pin20 (read is float value)
mdavis30 2:c3cb3ea3c9fa 32 AnalogIn ain(p18);
mdavis30 0:381a84fad08b 33
mdavis30 0:381a84fad08b 34 /* ************ These tickers work independent of any while loops ********** */
mdavis30 0:381a84fad08b 35 Ticker IMU_ticker; //ticker for printing IMU //https://developer.mbed.org/handbook/Ticker
mdavis30 0:381a84fad08b 36 Ticker BE_position_ticker; //probably delete soon
mdavis30 0:381a84fad08b 37 Ticker PRESSURE_ticker;
mdavis30 0:381a84fad08b 38
mdavis30 0:381a84fad08b 39 Ticker BCE_ticker; //new 6/5/17
mdavis30 3:1257a7d2eb3a 40 Ticker PID_ticker; //new 6/14/17
mdavis30 3:1257a7d2eb3a 41 Ticker LA_ticker; //new 6/22/17
mdavis30 3:1257a7d2eb3a 42
tnhnrl 5:7421776f6b08 43 float positionCmd = 200.0; //250
mdavis30 4:3c22d85a94a8 44
mdavis30 0:381a84fad08b 45 /* ************************************************************************* */
mdavis30 0:381a84fad08b 46
mdavis30 4:3c22d85a94a8 47 float pi = 3.14159265359;
mdavis30 4:3c22d85a94a8 48
mdavis30 3:1257a7d2eb3a 49 /* PID LOOP STUFF */
mdavis30 3:1257a7d2eb3a 50 float la_setPoint = 0.00; //the IMU pitch angle we want (setpoint)
mdavis30 3:1257a7d2eb3a 51
mdavis30 3:1257a7d2eb3a 52 float la_P_gain = 1.0;
mdavis30 3:1257a7d2eb3a 53 float la_I_gain = 0.00;
mdavis30 3:1257a7d2eb3a 54 float la_D_gain = 0.00;
mdavis30 3:1257a7d2eb3a 55 /* PID LOOP STUFF */
mdavis30 3:1257a7d2eb3a 56
mdavis30 3:1257a7d2eb3a 57 float IMU_pitch_angle = 0.00;
mdavis30 3:1257a7d2eb3a 58
mdavis30 3:1257a7d2eb3a 59 bool motor_retracting = false;
mdavis30 3:1257a7d2eb3a 60 bool motor_extending = false;
mdavis30 3:1257a7d2eb3a 61
tnhnrl 5:7421776f6b08 62 // 7/10/17
tnhnrl 5:7421776f6b08 63 string actual_position_string = "";
tnhnrl 5:7421776f6b08 64 double double_actual_position = 0.00;
tnhnrl 5:7421776f6b08 65
mdavis30 0:381a84fad08b 66 void IMU_ticking()
mdavis30 0:381a84fad08b 67 {
mdavis30 0:381a84fad08b 68 led1 = !led1; //flash the IMU LED
tnhnrl 5:7421776f6b08 69
tnhnrl 5:7421776f6b08 70 if (debug_mode)
tnhnrl 5:7421776f6b08 71 PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it
mdavis30 0:381a84fad08b 72 }
mdavis30 0:381a84fad08b 73
mdavis30 0:381a84fad08b 74 void PRESSURE_ticking()
mdavis30 0:381a84fad08b 75 {
tnhnrl 5:7421776f6b08 76 if (debug_mode)
tnhnrl 5:7421776f6b08 77 PC.printf("ressure: %f psi \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845); //read the analog pin
mdavis30 3:1257a7d2eb3a 78 //this voltage has been checked and scaled properly (6/28/2017)
mdavis30 0:381a84fad08b 79 }
mdavis30 0:381a84fad08b 80
mdavis30 0:381a84fad08b 81 void BCE_ticking() //new 6/5/17
mdavis30 0:381a84fad08b 82 {
tnhnrl 5:7421776f6b08 83 if (debug_mode)
tnhnrl 5:7421776f6b08 84 PC.printf("Buoyancy_Engine_POS: %3.0f mm BE_vel: %2.2f mm/s Set Point %3.0f posCon.getOutput: % 1.3f \n", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput());
tnhnrl 5:7421776f6b08 85
tnhnrl 5:7421776f6b08 86 //PC.printf("BE_pos: %3.0f mm BE_vel: %2.2f mm/s Set Point %3.0f controller output: % 1.3f \n", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput());
mdavis30 0:381a84fad08b 87 }
mdavis30 0:381a84fad08b 88
mdavis30 0:381a84fad08b 89 int main()
mdavis30 0:381a84fad08b 90 {
mdavis30 3:1257a7d2eb3a 91 PC.baud(9600); //mbed to PC serial connection speed
mdavis30 0:381a84fad08b 92 //PC.baud(230400);
mdavis30 0:381a84fad08b 93 //got screwy when i changed it
mdavis30 0:381a84fad08b 94 hBridge().stop();
mdavis30 3:1257a7d2eb3a 95
tnhnrl 5:7421776f6b08 96 PC.printf("* * * * * * * * * * * * * * * * *\n");
tnhnrl 5:7421776f6b08 97 PC.printf("PV TEST PROGRAM STARTED 7/13/2017\n");
tnhnrl 5:7421776f6b08 98 PC.printf("* * * * * * * * * * * * * * * * *\n");
mdavis30 3:1257a7d2eb3a 99
mdavis30 3:1257a7d2eb3a 100 systemTime().start(); //start the timer, needed for PID loop
mdavis30 3:1257a7d2eb3a 101
mdavis30 3:1257a7d2eb3a 102 /* **************** Linear Actuator MOTOR CONTROLLER **************** */
mdavis30 3:1257a7d2eb3a 103 Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class
mdavis30 3:1257a7d2eb3a 104
mdavis30 3:1257a7d2eb3a 105 PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled
mdavis30 3:1257a7d2eb3a 106
mdavis30 3:1257a7d2eb3a 107 PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor
mdavis30 3:1257a7d2eb3a 108 wait(1);
mdavis30 3:1257a7d2eb3a 109 PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor
mdavis30 3:1257a7d2eb3a 110 wait(1);
mdavis30 3:1257a7d2eb3a 111
mdavis30 0:381a84fad08b 112 //setup and start the adc. This runs on a fixed interval and is interrupt driven
mdavis30 0:381a84fad08b 113 adc().initialize();
mdavis30 0:381a84fad08b 114 adc().start();
mdavis30 0:381a84fad08b 115
mdavis30 0:381a84fad08b 116 //Initialize the position velocity filter. This will consume a couple of seconds for
mdavis30 0:381a84fad08b 117 //the filter to converge
mdavis30 0:381a84fad08b 118 pvf().init();
mdavis30 0:381a84fad08b 119
mdavis30 0:381a84fad08b 120 ////CHANGED TO GLOBAL VARIABLES
mdavis30 0:381a84fad08b 121 float motor_cmd = 0.0;
mdavis30 0:381a84fad08b 122 // float positionCmd = 250.0;
mdavis30 0:381a84fad08b 123 float P = 0.10;
mdavis30 0:381a84fad08b 124 float I = 0.00;
mdavis30 0:381a84fad08b 125 float D = 0.00;
mdavis30 0:381a84fad08b 126 float count = 0.0;
mdavis30 0:381a84fad08b 127 //char userInput; //from Trent's code?
mdavis30 0:381a84fad08b 128
mdavis30 4:3c22d85a94a8 129 float la_step = 1.0;
mdavis30 4:3c22d85a94a8 130 float la_setPoint_temp = 0.0;
mdavis30 4:3c22d85a94a8 131
tnhnrl 5:7421776f6b08 132 //Start off in manual mode for testing.
tnhnrl 5:7421776f6b08 133 bool BCE_auto = false;
tnhnrl 5:7421776f6b08 134 bool LA_auto = false;
mdavis30 4:3c22d85a94a8 135
mdavis30 4:3c22d85a94a8 136 float bce_auto_step_raw = 1.0;
mdavis30 4:3c22d85a94a8 137 float bce_auto_step_l;
tnhnrl 5:7421776f6b08 138 //float convert = 10000;
tnhnrl 5:7421776f6b08 139 float convert = 1;
mdavis30 4:3c22d85a94a8 140 float bce_auto_step_ml = bce_auto_step_raw * convert;
tnhnrl 5:7421776f6b08 141 int bce_manual_step = 10;
tnhnrl 5:7421776f6b08 142 //float volume_bce = 90.0*convert; //Troy: Not sure I get the conversion
tnhnrl 5:7421776f6b08 143 float volume_bce = 0;
mdavis30 4:3c22d85a94a8 144 float positionCmd_temp;
tnhnrl 5:7421776f6b08 145 //float ml_to_l= 0.000000001; //Is this a milliliter? TROY: 7/10/17
tnhnrl 5:7421776f6b08 146 float ml_to_l= 0.001; //milliliter???? TROY: 7/10/17
mdavis30 3:1257a7d2eb3a 147
mdavis30 0:381a84fad08b 148 hBridge().run(motor_cmd);
mdavis30 0:381a84fad08b 149
mdavis30 0:381a84fad08b 150 //set the intial gains for the position controller
mdavis30 0:381a84fad08b 151 posCon().setPgain(P);
mdavis30 0:381a84fad08b 152 posCon().setIgain(I);
mdavis30 0:381a84fad08b 153 posCon().setDgain(D);
tnhnrl 5:7421776f6b08 154 posCon().writeSetPoint(positionCmd); //7/13/17 initialize the position of the motor to 200
tnhnrl 5:7421776f6b08 155
tnhnrl 5:7421776f6b08 156 PC.printf("BCE Test Program Started!\n");
tnhnrl 5:7421776f6b08 157 wait(1);
mdavis30 0:381a84fad08b 158
mdavis30 0:381a84fad08b 159 /* *************************** LED *************************** */
mdavis30 0:381a84fad08b 160 led1 = 1; //initial values
mdavis30 0:381a84fad08b 161 led2 = 1;
tnhnrl 5:7421776f6b08 162 led3 = 0;
mdavis30 0:381a84fad08b 163 led4 = 1;
mdavis30 0:381a84fad08b 164 /* *************************** LED *************************** */
tnhnrl 5:7421776f6b08 165
tnhnrl 5:7421776f6b08 166 int la_cases = 0;
mdavis30 4:3c22d85a94a8 167 int count_while = 0;
mdavis30 4:3c22d85a94a8 168 //hBridge().reset();
mdavis30 4:3c22d85a94a8 169 PC.printf("\n```````````Linear Actuator in IMU controlled mode````````````\n\n");
mdavis30 0:381a84fad08b 170 //PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n");
mdavis30 0:381a84fad08b 171
tnhnrl 5:7421776f6b08 172 /* ************************** Pressure Sensor ************************** */
mdavis30 4:3c22d85a94a8 173 PRESSURE_ticker.attach(&PRESSURE_ticking, 3.0);
tnhnrl 5:7421776f6b08 174 /* ************************** Pressure Sensor ************************** */
mdavis30 0:381a84fad08b 175
mdavis30 0:381a84fad08b 176 /* *************************** MOTOR CONTROLLER *************************** */
mdavis30 3:1257a7d2eb3a 177 //Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class
mdavis30 0:381a84fad08b 178 /* *************************** MOTOR CONTROLLER *************************** */
mdavis30 0:381a84fad08b 179
mdavis30 0:381a84fad08b 180 /* *************************** IMU *************************** */
mdavis30 0:381a84fad08b 181 IMU_code IMU_object; //create the IMU object from the imported class
mdavis30 4:3c22d85a94a8 182 IMU_ticker.attach(&IMU_ticking, 3.0);
mdavis30 0:381a84fad08b 183 /* *************************** IMU *************************** */
mdavis30 0:381a84fad08b 184
mdavis30 0:381a84fad08b 185 /* *************************** BCE *************************** */
mdavis30 3:1257a7d2eb3a 186 //float previous_positionCmd = -1;
tnhnrl 5:7421776f6b08 187 //BCE_ticker.attach(&BCE_ticking, 3.0);
mdavis30 0:381a84fad08b 188 /* *************************** BCE *************************** */
mdavis30 0:381a84fad08b 189
mdavis30 0:381a84fad08b 190 while(1)
mdavis30 0:381a84fad08b 191 {
tnhnrl 5:7421776f6b08 192 //PC.printf("DEBUG: POT position: %f velocity: %f adc_count: %d VS conv_distance: %f adc_ch0_filter: %f\n ", pvf().getPosition(), pvf().getVelocity(), adc().ch0_filt, pvf().getVelocity(), adc().get_ch0_filt); //DEBUG TROY
tnhnrl 5:7421776f6b08 193 if (debug_mode)
tnhnrl 5:7421776f6b08 194 {
tnhnrl 5:7421776f6b08 195 PC.printf("DEBUG: POT position: %6.3f velocity: %6.3f adc_count: %d VS conv_distance: %6.3f adc_ch0_filter: %6.3f\n", pvf().getPosition(), pvf().getVelocity(), adc().ch0_filt, pvf().get_conv_distance(), adc().get_ch0_filt()); //DEBUG TROY
tnhnrl 5:7421776f6b08 196 PC.printf("DEBUG: dt: %f current_time: %f last_time: %f\n", pvf().getDt(), pvf().get_curr_time(), pvf().get_last_time()); //DEBUG TROY
tnhnrl 5:7421776f6b08 197 PC.printf("DEBUG: x1: %f x2: %f x1_dot: %f x2_dot: %f\n", pvf().get_x1(), pvf().get_x2(), pvf().get_x1_dot(), pvf().get_x2_dot()); //DEBUG TROY
tnhnrl 5:7421776f6b08 198 }
tnhnrl 5:7421776f6b08 199
mdavis30 0:381a84fad08b 200 /* *************************** IMU *************************** */
mdavis30 0:381a84fad08b 201 IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop
tnhnrl 5:7421776f6b08 202 IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly?
tnhnrl 5:7421776f6b08 203
tnhnrl 5:7421776f6b08 204 if (debug_mode)
tnhnrl 5:7421776f6b08 205 PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_pitch_angle, la_setPoint);
mdavis30 0:381a84fad08b 206 /* *************************** IMU *************************** */
mdavis30 0:381a84fad08b 207
mdavis30 0:381a84fad08b 208 /* Buoyancy Engine */
mdavis30 0:381a84fad08b 209 // update the position velocity filter
mdavis30 0:381a84fad08b 210 pvf().update();
mdavis30 0:381a84fad08b 211
mdavis30 0:381a84fad08b 212 //update the controller with the current numbers in the position guesser
mdavis30 0:381a84fad08b 213 posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ;
mdavis30 0:381a84fad08b 214 hBridge().run(posCon().getOutput());
mdavis30 0:381a84fad08b 215
mdavis30 0:381a84fad08b 216 /* Buoyancy Engine */
mdavis30 0:381a84fad08b 217
mdavis30 0:381a84fad08b 218 //FOR DEBUGGING
mdavis30 0:381a84fad08b 219 //PC.printf("BE_pos: %3.0f mm BE_vel: %2.2f mm/s Set Point %3.0f controller output: % 1.3f P: %1.3f I: %1.4f D: %1.4f\r", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput(), P, I, D);
mdavis30 0:381a84fad08b 220
mdavis30 0:381a84fad08b 221 if (PC.readable())
mdavis30 0:381a84fad08b 222 {
tnhnrl 5:7421776f6b08 223 led4 = !led4; //read indicator changes
tnhnrl 5:7421776f6b08 224 Key=PC.getc();
mdavis30 0:381a84fad08b 225
mdavis30 4:3c22d85a94a8 226 //Universal MBED Controls
tnhnrl 5:7421776f6b08 227 if(Key == '!') //RESET THE MBED
mdavis30 0:381a84fad08b 228 {
mdavis30 4:3c22d85a94a8 229 PC.printf("MBED RESET KEY (!) PRESSED\n");
mdavis30 4:3c22d85a94a8 230 PC.printf("Linear Actuator Motor disabled!\n");
mdavis30 4:3c22d85a94a8 231 //disable the motor
mdavis30 4:3c22d85a94a8 232 BLA_object.Keyboard_Q(); //DISABLE THE MOTOR
mdavis30 4:3c22d85a94a8 233 wait(0.5); //500 milliseconds
mdavis30 4:3c22d85a94a8 234 mbed_reset(); //reset the mbed!
mdavis30 0:381a84fad08b 235 }
tnhnrl 5:7421776f6b08 236
tnhnrl 5:7421776f6b08 237 else if(Key == '~') // (shift + '`')
tnhnrl 5:7421776f6b08 238 {
tnhnrl 5:7421776f6b08 239 debug_mode = !debug_mode; //shift it back and forth
tnhnrl 5:7421776f6b08 240 PC.printf(" # # # # # # DEBUG MODE: %d # # # # # # \n", debug_mode);
tnhnrl 5:7421776f6b08 241 }
tnhnrl 5:7421776f6b08 242
tnhnrl 5:7421776f6b08 243 else if(Key == 'H') //homing sequence
mdavis30 0:381a84fad08b 244 {
mdavis30 4:3c22d85a94a8 245 PC.printf("### homing the device ###");
mdavis30 4:3c22d85a94a8 246 BLA_object.Keyboard_H();
mdavis30 4:3c22d85a94a8 247 wait(5); //for debugging
mdavis30 4:3c22d85a94a8 248
tnhnrl 5:7421776f6b08 249 ////TEST THIS
tnhnrl 5:7421776f6b08 250 //PC.printf("BE_pos: 0\n");
tnhnrl 5:7421776f6b08 251 //PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str()); //flip this back and forth
tnhnrl 5:7421776f6b08 252
tnhnrl 5:7421776f6b08 253 //TROY: TEST THIS 7/11/2017
tnhnrl 5:7421776f6b08 254 const char *char_actual_position = BLA_object.get_pos().c_str();
tnhnrl 5:7421776f6b08 255 //actual_position_string = BLA_object.get_pos().c_str();
tnhnrl 5:7421776f6b08 256
tnhnrl 5:7421776f6b08 257 sscanf(char_actual_position, "%lf", &double_actual_position);
tnhnrl 5:7421776f6b08 258 // 7/10/17
tnhnrl 5:7421776f6b08 259
tnhnrl 5:7421776f6b08 260 PC.printf("### position is\nBEP: %lf ###\n", double_actual_position);
tnhnrl 5:7421776f6b08 261
mdavis30 4:3c22d85a94a8 262 wait(1); //for debugging
mdavis30 4:3c22d85a94a8 263 }
tnhnrl 5:7421776f6b08 264 else if(Key == 'p' or Key == 'P')
mdavis30 3:1257a7d2eb3a 265 {
tnhnrl 5:7421776f6b08 266 // 7/10/17
tnhnrl 5:7421776f6b08 267 //actual_position_string = BLA_object.get_pos();
tnhnrl 5:7421776f6b08 268 //actual_position_string = BLA_object.get_pos().c_str();
tnhnrl 5:7421776f6b08 269 //const char *char_actual_position = string_actual_position.c_str();
tnhnrl 5:7421776f6b08 270 const char *char_actual_position = BLA_object.get_pos().c_str();
tnhnrl 5:7421776f6b08 271 //actual_position_string = BLA_object.get_pos().c_str();
tnhnrl 5:7421776f6b08 272
tnhnrl 5:7421776f6b08 273 sscanf(char_actual_position, "%lf", &double_actual_position);
tnhnrl 5:7421776f6b08 274 // 7/10/17
tnhnrl 5:7421776f6b08 275
tnhnrl 5:7421776f6b08 276 PC.printf("### position is\nBEP: %lf ###\n", double_actual_position); //flip this back and forth
mdavis30 4:3c22d85a94a8 277 wait(1); //for debugging
mdavis30 3:1257a7d2eb3a 278 // "-999999" means it is not working
mdavis30 3:1257a7d2eb3a 279 }
mdavis30 4:3c22d85a94a8 280
mdavis30 4:3c22d85a94a8 281 //Buoyancy Engine Controls
mdavis30 4:3c22d85a94a8 282 else if (Key == ',' or Key == '<')
mdavis30 4:3c22d85a94a8 283 {
mdavis30 4:3c22d85a94a8 284 if (BCE_auto == false)
mdavis30 4:3c22d85a94a8 285 {
mdavis30 4:3c22d85a94a8 286 PC.printf("BCE: Now in Automatic Mode\n");
mdavis30 4:3c22d85a94a8 287 BCE_auto = true;
mdavis30 4:3c22d85a94a8 288 }
mdavis30 4:3c22d85a94a8 289 else
mdavis30 4:3c22d85a94a8 290 {
tnhnrl 5:7421776f6b08 291 PC.printf("BCE: Still in Automatic Mode\n");
mdavis30 4:3c22d85a94a8 292 }
mdavis30 4:3c22d85a94a8 293 }
mdavis30 4:3c22d85a94a8 294 else if (Key == '.' or Key == '>')
mdavis30 4:3c22d85a94a8 295 {
mdavis30 4:3c22d85a94a8 296 if (BCE_auto == true)
mdavis30 4:3c22d85a94a8 297 {
mdavis30 4:3c22d85a94a8 298 PC.printf("BCE: Now in Manual Mode\n");
mdavis30 4:3c22d85a94a8 299 BCE_auto = false;
mdavis30 4:3c22d85a94a8 300 }
mdavis30 4:3c22d85a94a8 301 else
mdavis30 4:3c22d85a94a8 302 {
tnhnrl 5:7421776f6b08 303 PC.printf("BCE: Still in Manual Mode\n");
mdavis30 4:3c22d85a94a8 304 }
mdavis30 4:3c22d85a94a8 305 }
mdavis30 4:3c22d85a94a8 306 //BCE Automatic Controls
mdavis30 4:3c22d85a94a8 307 else if (Key == 's' or Key == 'S')
mdavis30 4:3c22d85a94a8 308 {
mdavis30 4:3c22d85a94a8 309 if (BCE_auto == true)
mdavis30 4:3c22d85a94a8 310 {
mdavis30 4:3c22d85a94a8 311 //PC.printf("BCE Automatic Step Size Change\n");
mdavis30 4:3c22d85a94a8 312 if (bce_auto_step_raw == 1.0)
mdavis30 4:3c22d85a94a8 313 {
mdavis30 4:3c22d85a94a8 314 bce_auto_step_raw = 5.0;
mdavis30 4:3c22d85a94a8 315 }
mdavis30 4:3c22d85a94a8 316 else if (bce_auto_step_raw == 5.0)
mdavis30 4:3c22d85a94a8 317 {
mdavis30 4:3c22d85a94a8 318 bce_auto_step_raw = 10.0;
mdavis30 4:3c22d85a94a8 319 }
mdavis30 4:3c22d85a94a8 320 else if (bce_auto_step_raw == 10.0)
mdavis30 4:3c22d85a94a8 321 {
tnhnrl 5:7421776f6b08 322 bce_auto_step_raw = 50.0;
tnhnrl 5:7421776f6b08 323 }
tnhnrl 5:7421776f6b08 324 else if (bce_auto_step_raw == 50.0)
tnhnrl 5:7421776f6b08 325 {
tnhnrl 5:7421776f6b08 326 bce_auto_step_raw = 100.0;
tnhnrl 5:7421776f6b08 327 }
tnhnrl 5:7421776f6b08 328 else if (bce_auto_step_raw == 100.0)
tnhnrl 5:7421776f6b08 329 {
mdavis30 4:3c22d85a94a8 330 bce_auto_step_raw = 1.0;
mdavis30 4:3c22d85a94a8 331 }
mdavis30 4:3c22d85a94a8 332 bce_auto_step_ml = bce_auto_step_raw * convert;
tnhnrl 5:7421776f6b08 333 PC.printf("BCE Auto Step Size Now\n BE_ST_ML: %7.0f milliliters\n", bce_auto_step_ml);
mdavis30 4:3c22d85a94a8 334 }
mdavis30 4:3c22d85a94a8 335 else
mdavis30 4:3c22d85a94a8 336 {
mdavis30 4:3c22d85a94a8 337 PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
mdavis30 4:3c22d85a94a8 338
mdavis30 4:3c22d85a94a8 339 }
mdavis30 4:3c22d85a94a8 340 }
tnhnrl 5:7421776f6b08 341 else if(Key == 'd' or Key == 'D') //0 mm = 0 mL, 350 mm = 1816 mL
mdavis30 4:3c22d85a94a8 342 {
tnhnrl 5:7421776f6b08 343 PC.printf("(d) volume_bce: %f\n", volume_bce);
mdavis30 4:3c22d85a94a8 344 if (BCE_auto == true)
mdavis30 4:3c22d85a94a8 345 {
tnhnrl 5:7421776f6b08 346 PC.printf("(d) BCE_auto: %d\n", BCE_auto);
tnhnrl 5:7421776f6b08 347 if (volume_bce >= 1) //350 mm retracted from end = 1816 mL in buyoancy engine tube
tnhnrl 5:7421776f6b08 348 {
tnhnrl 5:7421776f6b08 349 volume_bce -= bce_auto_step_ml;
tnhnrl 5:7421776f6b08 350 float calc_height = (volume_bce * 1000) / (pi*40.64*40.64);
tnhnrl 5:7421776f6b08 351 PC.printf("Buoyancy Engine Volume VBE: %1.5f milliliters (Distance: %f mm)\n", volume_bce, calc_height); //to read in MATLAB
tnhnrl 5:7421776f6b08 352 }
tnhnrl 5:7421776f6b08 353 else if (volume_bce < 1)
tnhnrl 5:7421776f6b08 354 {
tnhnrl 5:7421776f6b08 355 PC.printf("Volume reset to 1 mL!\n"); //keep the volume at zero mL
tnhnrl 5:7421776f6b08 356 volume_bce = 1;
tnhnrl 5:7421776f6b08 357 }
mdavis30 4:3c22d85a94a8 358 }
mdavis30 4:3c22d85a94a8 359 else
mdavis30 4:3c22d85a94a8 360 {
mdavis30 4:3c22d85a94a8 361 PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
mdavis30 4:3c22d85a94a8 362 }
mdavis30 4:3c22d85a94a8 363 }
mdavis30 4:3c22d85a94a8 364 else if(Key == 'f' or Key == 'F')
mdavis30 4:3c22d85a94a8 365 {
tnhnrl 5:7421776f6b08 366 PC.printf("(f) volume_bce: %f\n", volume_bce);
mdavis30 4:3c22d85a94a8 367 if (BCE_auto == true)
mdavis30 4:3c22d85a94a8 368 {
tnhnrl 5:7421776f6b08 369 PC.printf("(f) BCE_auto: %d\n", BCE_auto);
tnhnrl 5:7421776f6b08 370 if (volume_bce <= 1816) //350 mm retracted from end = 1816 mL in buyoancy engine tube
tnhnrl 5:7421776f6b08 371 {
tnhnrl 5:7421776f6b08 372 volume_bce += bce_auto_step_ml;
tnhnrl 5:7421776f6b08 373 float calc_height = (volume_bce * 1000) / (pi*40.64*40.64);
tnhnrl 5:7421776f6b08 374 PC.printf("Buoyancy Engine Volume VBE: %1.5f milliliters (Distance: %f mm)\n", volume_bce, calc_height); //to read in MATLAB
tnhnrl 5:7421776f6b08 375 }
tnhnrl 5:7421776f6b08 376 else if (volume_bce > 1816)
tnhnrl 5:7421776f6b08 377 {
tnhnrl 5:7421776f6b08 378 PC.printf("Volume reset to 1816 mL (max volume)!\n"); //keep the volume at 1816 mL (max)
tnhnrl 5:7421776f6b08 379 volume_bce = 1816;
tnhnrl 5:7421776f6b08 380 }
mdavis30 4:3c22d85a94a8 381 }
mdavis30 4:3c22d85a94a8 382 else
mdavis30 4:3c22d85a94a8 383 {
mdavis30 4:3c22d85a94a8 384 PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
mdavis30 4:3c22d85a94a8 385 }
mdavis30 4:3c22d85a94a8 386 }
mdavis30 4:3c22d85a94a8 387 else if(Key == 'r' or Key == 'R')
mdavis30 4:3c22d85a94a8 388 {
mdavis30 4:3c22d85a94a8 389 if (BCE_auto == true)
mdavis30 4:3c22d85a94a8 390 {
mdavis30 4:3c22d85a94a8 391 PC.printf("\nR received!\n");
tnhnrl 5:7421776f6b08 392
tnhnrl 5:7421776f6b08 393 //Troy equation (volume 1 mL = 1000 mm^3)
tnhnrl 5:7421776f6b08 394 positionCmd = (volume_bce * 1000) / (pi*40.64*40.64); //volume / (pi * r^2)
tnhnrl 5:7421776f6b08 395 PC.printf("\nBCE volume sent was %d mL (distance: %f mm)\n", volume_bce, positionCmd);
tnhnrl 5:7421776f6b08 396 PC.printf("(BCE Distance) VBE_SENT: %3.0f\n", positionCmd);
tnhnrl 5:7421776f6b08 397
tnhnrl 5:7421776f6b08 398 posCon().writeSetPoint(positionCmd); //write the setPoint (target)
tnhnrl 5:7421776f6b08 399
tnhnrl 5:7421776f6b08 400 hBridge().run(posCon().getOutput()); //run the h-bridge until it reaches the target
mdavis30 4:3c22d85a94a8 401
tnhnrl 5:7421776f6b08 402 hBridge().reset(); //reset to start this process of moving the h-bridge
mdavis30 4:3c22d85a94a8 403
tnhnrl 5:7421776f6b08 404 count = 0; //not sure...
mdavis30 4:3c22d85a94a8 405 }
mdavis30 4:3c22d85a94a8 406 else
mdavis30 4:3c22d85a94a8 407 {
mdavis30 4:3c22d85a94a8 408 PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
mdavis30 4:3c22d85a94a8 409 }
mdavis30 4:3c22d85a94a8 410 }
mdavis30 4:3c22d85a94a8 411 //BCE Manual Controls
mdavis30 4:3c22d85a94a8 412 else if (Key == '2' or Key == '@')
mdavis30 4:3c22d85a94a8 413 {
mdavis30 4:3c22d85a94a8 414 if (BCE_auto == false)
mdavis30 4:3c22d85a94a8 415 {
mdavis30 4:3c22d85a94a8 416 PC.printf("BCE Manual Step Size Change\n");
tnhnrl 5:7421776f6b08 417 if (bce_manual_step == 1)
mdavis30 4:3c22d85a94a8 418 {
tnhnrl 5:7421776f6b08 419 bce_manual_step = 10;
mdavis30 4:3c22d85a94a8 420 }
tnhnrl 5:7421776f6b08 421 else if (bce_manual_step == 10)
mdavis30 4:3c22d85a94a8 422 {
tnhnrl 5:7421776f6b08 423 bce_manual_step = 25;
mdavis30 4:3c22d85a94a8 424 }
tnhnrl 5:7421776f6b08 425 else if (bce_manual_step == 25)
mdavis30 4:3c22d85a94a8 426 {
tnhnrl 5:7421776f6b08 427 bce_manual_step = 50;
mdavis30 4:3c22d85a94a8 428 }
tnhnrl 5:7421776f6b08 429 else if (bce_manual_step == 50)
mdavis30 4:3c22d85a94a8 430 {
tnhnrl 5:7421776f6b08 431 bce_manual_step = 1;
mdavis30 4:3c22d85a94a8 432 }
mdavis30 4:3c22d85a94a8 433
tnhnrl 5:7421776f6b08 434 PC.printf("BCE Manual Step Size Now\nBEM_ST: %d\n", bce_manual_step);
tnhnrl 5:7421776f6b08 435 }
tnhnrl 5:7421776f6b08 436 else
tnhnrl 5:7421776f6b08 437 {
tnhnrl 5:7421776f6b08 438 PC.printf("ERROR: BCE in Auto Mode, this is a manual command\n");
tnhnrl 5:7421776f6b08 439 }
tnhnrl 5:7421776f6b08 440 }
tnhnrl 5:7421776f6b08 441 else if (Key == 'z' or Key == 'Z')
tnhnrl 5:7421776f6b08 442 {
tnhnrl 5:7421776f6b08 443 if (BCE_auto == false)
tnhnrl 5:7421776f6b08 444 {
tnhnrl 5:7421776f6b08 445 positionCmd -= bce_manual_step;
tnhnrl 5:7421776f6b08 446 //PC.printf(">>> BEM_P: %3.0f\n", positionCmd); //to read in MATLAB (DEBUG)
tnhnrl 5:7421776f6b08 447
tnhnrl 5:7421776f6b08 448 //decrement the duty cycle
tnhnrl 5:7421776f6b08 449 if (positionCmd >= 50 && positionCmd <=350) //limit buoyancy engine position 25 to 375
tnhnrl 5:7421776f6b08 450 {
tnhnrl 5:7421776f6b08 451 PC.printf("Commanded BCE position is BEM_P: %3.0f\n", positionCmd); //to read in MATLAB
tnhnrl 5:7421776f6b08 452 }
tnhnrl 5:7421776f6b08 453 else if (positionCmd < 50)
tnhnrl 5:7421776f6b08 454 {
tnhnrl 5:7421776f6b08 455 PC.printf("BCE past limits! Reset to 50\n"); //to read in MATLAB
tnhnrl 5:7421776f6b08 456 positionCmd = 50;
tnhnrl 5:7421776f6b08 457 }
tnhnrl 5:7421776f6b08 458 }
tnhnrl 5:7421776f6b08 459 else
tnhnrl 5:7421776f6b08 460 {
tnhnrl 5:7421776f6b08 461 PC.printf("ERROR: BCE in Auto Mode, this is a manual command\n");
tnhnrl 5:7421776f6b08 462 }
tnhnrl 5:7421776f6b08 463 }
tnhnrl 5:7421776f6b08 464 else if (Key == 'l' or Key == 'L')
tnhnrl 5:7421776f6b08 465 PC.printf("DEBUG: String position? %f velocity? %f (BCE active: %d)\n", pvf().getPosition(), pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY
tnhnrl 5:7421776f6b08 466
tnhnrl 5:7421776f6b08 467 else if (Key == 'x' or Key == 'X')
tnhnrl 5:7421776f6b08 468 {
tnhnrl 5:7421776f6b08 469 if (BCE_auto == false)
tnhnrl 5:7421776f6b08 470 {
tnhnrl 5:7421776f6b08 471 positionCmd += bce_manual_step;
tnhnrl 5:7421776f6b08 472 //PC.printf(">>> BEM_P: %3.0f\n", positionCmd); //to read in MATLAB (DEBUG)
tnhnrl 5:7421776f6b08 473
tnhnrl 5:7421776f6b08 474 //decrement the duty cycle
tnhnrl 5:7421776f6b08 475 if (positionCmd >= 50 && positionCmd <=350) //limit buoyancy engine position 25 to 375
tnhnrl 5:7421776f6b08 476 {
tnhnrl 5:7421776f6b08 477 PC.printf("Commanded BCE position is BEM_P: %3.0f\n", positionCmd); //to read in MATLAB
tnhnrl 5:7421776f6b08 478 }
tnhnrl 5:7421776f6b08 479 else if (positionCmd >350)
tnhnrl 5:7421776f6b08 480 {
tnhnrl 5:7421776f6b08 481 PC.printf("BCE past limits! Reset to 350\n"); //to read in MATLAB
tnhnrl 5:7421776f6b08 482 positionCmd = 350;
tnhnrl 5:7421776f6b08 483 }
mdavis30 4:3c22d85a94a8 484 }
mdavis30 4:3c22d85a94a8 485 else
mdavis30 4:3c22d85a94a8 486 {
mdavis30 4:3c22d85a94a8 487 PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
mdavis30 4:3c22d85a94a8 488 }
mdavis30 4:3c22d85a94a8 489 }
tnhnrl 5:7421776f6b08 490 else if(Key == 'w' or Key == 'W')
mdavis30 4:3c22d85a94a8 491 {
mdavis30 4:3c22d85a94a8 492 if (BCE_auto == false)
mdavis30 4:3c22d85a94a8 493 {
mdavis30 4:3c22d85a94a8 494 PC.printf("\nW received!\n");
tnhnrl 5:7421776f6b08 495 PC.printf("BEM_SND: %3.0f\n", positionCmd);
tnhnrl 5:7421776f6b08 496
tnhnrl 5:7421776f6b08 497 posCon().writeSetPoint(positionCmd); //writing once doesn't work sometimes
tnhnrl 5:7421776f6b08 498
mdavis30 4:3c22d85a94a8 499 //posCon().setPgain(P);
mdavis30 4:3c22d85a94a8 500 //posCon().setIgain(I);
mdavis30 4:3c22d85a94a8 501 //posCon().setDgain(D);
mdavis30 4:3c22d85a94a8 502 hBridge().run(posCon().getOutput());
mdavis30 4:3c22d85a94a8 503
mdavis30 4:3c22d85a94a8 504 hBridge().reset();
mdavis30 4:3c22d85a94a8 505
mdavis30 4:3c22d85a94a8 506 count = 0;
mdavis30 4:3c22d85a94a8 507
mdavis30 4:3c22d85a94a8 508 }
mdavis30 4:3c22d85a94a8 509 else
mdavis30 4:3c22d85a94a8 510 {
mdavis30 4:3c22d85a94a8 511 PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
mdavis30 4:3c22d85a94a8 512 }
mdavis30 4:3c22d85a94a8 513 }
mdavis30 4:3c22d85a94a8 514
mdavis30 4:3c22d85a94a8 515 //Linear Actuator Controls
mdavis30 4:3c22d85a94a8 516 else if(Key == 'c' or Key == 'C')
mdavis30 4:3c22d85a94a8 517 {
mdavis30 4:3c22d85a94a8 518 if (LA_auto == true)
mdavis30 4:3c22d85a94a8 519 {
mdavis30 4:3c22d85a94a8 520 PC.printf("ERROR: LA already in auto mode\n");
mdavis30 4:3c22d85a94a8 521 }
mdavis30 4:3c22d85a94a8 522 else
mdavis30 4:3c22d85a94a8 523 {
mdavis30 4:3c22d85a94a8 524 LA_auto = true;
tnhnrl 5:7421776f6b08 525 PC.printf("```````````LA now in IMU (Auto) Controlled Mode```````````````\n");
tnhnrl 5:7421776f6b08 526 la_cases = 0;
mdavis30 4:3c22d85a94a8 527 count_while = 0;
mdavis30 4:3c22d85a94a8 528 }
mdavis30 4:3c22d85a94a8 529 }
mdavis30 4:3c22d85a94a8 530 else if (Key == 'v' or Key == 'V')
mdavis30 4:3c22d85a94a8 531 {
mdavis30 4:3c22d85a94a8 532 if (LA_auto == true)
mdavis30 4:3c22d85a94a8 533 {
mdavis30 4:3c22d85a94a8 534 LA_auto = false;
mdavis30 4:3c22d85a94a8 535 //Change cases: go from imu controlled to manual
tnhnrl 5:7421776f6b08 536 PC.printf("```````````LA now in Manual Mode````````````````````\n");
tnhnrl 5:7421776f6b08 537 la_cases = 1;
mdavis30 4:3c22d85a94a8 538 count_while = 0;
mdavis30 4:3c22d85a94a8 539 }
mdavis30 4:3c22d85a94a8 540 else
mdavis30 4:3c22d85a94a8 541 {
mdavis30 4:3c22d85a94a8 542 PC.printf("ERROR: LA already in manual mode\n");
tnhnrl 5:7421776f6b08 543 PC.printf("LA_auto ==> %d\n", LA_auto); //should show "0" (false)
mdavis30 4:3c22d85a94a8 544 }
mdavis30 4:3c22d85a94a8 545 }
mdavis30 4:3c22d85a94a8 546 else if (Key == '0' or Key == ')')
mdavis30 4:3c22d85a94a8 547 {
tnhnrl 5:7421776f6b08 548 if (LA_auto == true)
mdavis30 4:3c22d85a94a8 549 {
tnhnrl 5:7421776f6b08 550 PC.printf(") recieved\n");
tnhnrl 5:7421776f6b08 551 if (la_step == 0.5)
tnhnrl 5:7421776f6b08 552 {
tnhnrl 5:7421776f6b08 553 la_step = 1.0;
tnhnrl 5:7421776f6b08 554 }
tnhnrl 5:7421776f6b08 555 else if (la_step == 1.0)
tnhnrl 5:7421776f6b08 556 {
tnhnrl 5:7421776f6b08 557 la_step = 5.0;
tnhnrl 5:7421776f6b08 558 }
tnhnrl 5:7421776f6b08 559 else if (la_step == 5.0)
tnhnrl 5:7421776f6b08 560 {
tnhnrl 5:7421776f6b08 561 la_step = 10.0;
tnhnrl 5:7421776f6b08 562 }
tnhnrl 5:7421776f6b08 563 else if (la_step == 10.0)
tnhnrl 5:7421776f6b08 564 {
tnhnrl 5:7421776f6b08 565 la_step = 15.0;
tnhnrl 5:7421776f6b08 566 }
tnhnrl 5:7421776f6b08 567 else if (la_step == 15.0)
tnhnrl 5:7421776f6b08 568 {
tnhnrl 5:7421776f6b08 569 la_step = 0.5;
tnhnrl 5:7421776f6b08 570 }
tnhnrl 5:7421776f6b08 571 PC.printf("LA Step Size Now\nLA_ST_SZ: %f\n", la_step);
mdavis30 4:3c22d85a94a8 572 }
tnhnrl 5:7421776f6b08 573 else
tnhnrl 5:7421776f6b08 574 {
tnhnrl 5:7421776f6b08 575 PC.printf("ERROR: LA in manual mode!\n");
mdavis30 4:3c22d85a94a8 576 }
tnhnrl 5:7421776f6b08 577 }
tnhnrl 5:7421776f6b08 578
tnhnrl 5:7421776f6b08 579 else if (Key == '-' or Key == '_')
tnhnrl 5:7421776f6b08 580 {
tnhnrl 5:7421776f6b08 581 if (LA_auto == true)
mdavis30 4:3c22d85a94a8 582 {
tnhnrl 5:7421776f6b08 583 la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0;
tnhnrl 5:7421776f6b08 584 PC.printf("- recieved\n");
tnhnrl 5:7421776f6b08 585 PC.printf("LA auto step size: %f\n", la_step);
tnhnrl 5:7421776f6b08 586 PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp);
mdavis30 4:3c22d85a94a8 587 }
tnhnrl 5:7421776f6b08 588 else
mdavis30 4:3c22d85a94a8 589 {
tnhnrl 5:7421776f6b08 590 PC.printf("ERROR: LA in manual mode!\n");
mdavis30 4:3c22d85a94a8 591 }
mdavis30 4:3c22d85a94a8 592 }
tnhnrl 5:7421776f6b08 593 else if (Key == '=' or Key == '+')
tnhnrl 5:7421776f6b08 594 {
tnhnrl 5:7421776f6b08 595 if (LA_auto == true)
tnhnrl 5:7421776f6b08 596 {
tnhnrl 5:7421776f6b08 597 la_setPoint_temp += la_step; //IMU_pitch_angle += 1.0;
tnhnrl 5:7421776f6b08 598 PC.printf("+ recieved\n");
tnhnrl 5:7421776f6b08 599 PC.printf("LA auto step size: %f\n", la_step);
tnhnrl 5:7421776f6b08 600 PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp);
tnhnrl 5:7421776f6b08 601 }
tnhnrl 5:7421776f6b08 602 else
tnhnrl 5:7421776f6b08 603 {
tnhnrl 5:7421776f6b08 604 PC.printf("ERROR: LA in manual mode!\n");
tnhnrl 5:7421776f6b08 605 }
tnhnrl 5:7421776f6b08 606 }
mdavis30 3:1257a7d2eb3a 607 else if (Key == 'A' or Key == 'a')
mdavis30 0:381a84fad08b 608 {
tnhnrl 5:7421776f6b08 609 if (LA_auto == true)
tnhnrl 5:7421776f6b08 610 {
tnhnrl 5:7421776f6b08 611 PC.printf("A recieved\n");
tnhnrl 5:7421776f6b08 612 la_setPoint=la_setPoint_temp;
tnhnrl 5:7421776f6b08 613 PC.printf("LA angle now set to\nLA_A_SND: %f\n", la_setPoint);
tnhnrl 5:7421776f6b08 614 }
tnhnrl 5:7421776f6b08 615 else
tnhnrl 5:7421776f6b08 616 {
tnhnrl 5:7421776f6b08 617 PC.printf("ERROR: LA in manual mode!\n");
tnhnrl 5:7421776f6b08 618 }
mdavis30 0:381a84fad08b 619 }
mdavis30 4:3c22d85a94a8 620
tnhnrl 5:7421776f6b08 621 else if (Key == '[' or Key == '{')
mdavis30 3:1257a7d2eb3a 622 {
mdavis30 3:1257a7d2eb3a 623 la_P_gain -= 0.1;
mdavis30 3:1257a7d2eb3a 624 PC.printf("[ key pressed\n");
mdavis30 3:1257a7d2eb3a 625 PC.printf("P gain is now %f\n", la_P_gain);
mdavis30 3:1257a7d2eb3a 626
mdavis30 3:1257a7d2eb3a 627 }
tnhnrl 5:7421776f6b08 628 else if (Key == ']' or Key == '}')
mdavis30 3:1257a7d2eb3a 629 {
mdavis30 3:1257a7d2eb3a 630 la_P_gain += 0.1;
mdavis30 3:1257a7d2eb3a 631 PC.printf("] key pressed\n");
mdavis30 3:1257a7d2eb3a 632 PC.printf("P gain is now %f\n", la_P_gain);
mdavis30 3:1257a7d2eb3a 633
mdavis30 3:1257a7d2eb3a 634 }
tnhnrl 5:7421776f6b08 635 else if (Key == ';')
mdavis30 3:1257a7d2eb3a 636 {
mdavis30 3:1257a7d2eb3a 637 la_I_gain -= 0.1;
mdavis30 4:3c22d85a94a8 638 PC.printf("; key pressed\n");
mdavis30 4:3c22d85a94a8 639 PC.printf("I gain is now %f\n", la_I_gain);
mdavis30 3:1257a7d2eb3a 640
mdavis30 0:381a84fad08b 641
mdavis30 0:381a84fad08b 642 }
tnhnrl 5:7421776f6b08 643 else if (Key == '\'')
mdavis30 4:3c22d85a94a8 644 {
mdavis30 3:1257a7d2eb3a 645 la_I_gain += 0.1;
tnhnrl 5:7421776f6b08 646 PC.printf("\\ key pressed\n");
mdavis30 4:3c22d85a94a8 647 PC.printf("I gain is now %f\n", la_I_gain);
mdavis30 4:3c22d85a94a8 648 }
tnhnrl 5:7421776f6b08 649 else if (Key == '.')
mdavis30 0:381a84fad08b 650 {
mdavis30 4:3c22d85a94a8 651 la_D_gain -= 0.1;
mdavis30 4:3c22d85a94a8 652 PC.printf(". key pressed\n");
mdavis30 4:3c22d85a94a8 653 PC.printf("D gain is now %f\n", la_D_gain);
mdavis30 4:3c22d85a94a8 654 }
tnhnrl 5:7421776f6b08 655 else if (Key == '/')
mdavis30 4:3c22d85a94a8 656 {
mdavis30 4:3c22d85a94a8 657 la_D_gain += 0.1;
mdavis30 4:3c22d85a94a8 658 PC.printf("/ key pressed\n");
mdavis30 4:3c22d85a94a8 659 PC.printf("D gain is now %f\n", la_D_gain);
mdavis30 4:3c22d85a94a8 660 }
mdavis30 4:3c22d85a94a8 661
mdavis30 4:3c22d85a94a8 662 else if(Key == 'n' or Key == 'N')
mdavis30 0:381a84fad08b 663 {
tnhnrl 5:7421776f6b08 664 if (LA_auto == false)
tnhnrl 5:7421776f6b08 665 {
tnhnrl 5:7421776f6b08 666 PC.printf("N key pressed. \n");
tnhnrl 5:7421776f6b08 667 PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY());
tnhnrl 5:7421776f6b08 668 }
tnhnrl 5:7421776f6b08 669
tnhnrl 5:7421776f6b08 670 else
tnhnrl 5:7421776f6b08 671 {
tnhnrl 5:7421776f6b08 672 PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
tnhnrl 5:7421776f6b08 673 }
mdavis30 4:3c22d85a94a8 674 }
mdavis30 4:3c22d85a94a8 675 else if(Key == 'm' or Key == 'M')
mdavis30 4:3c22d85a94a8 676 {
tnhnrl 5:7421776f6b08 677 if (LA_auto == false)
tnhnrl 5:7421776f6b08 678 {
tnhnrl 5:7421776f6b08 679 PC.printf("M key pressed. \n");
tnhnrl 5:7421776f6b08 680 PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY());
tnhnrl 5:7421776f6b08 681 }
tnhnrl 5:7421776f6b08 682
tnhnrl 5:7421776f6b08 683 else
tnhnrl 5:7421776f6b08 684 {
tnhnrl 5:7421776f6b08 685 PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
tnhnrl 5:7421776f6b08 686 }
mdavis30 0:381a84fad08b 687 }
mdavis30 4:3c22d85a94a8 688 else if(Key == 'j' or Key == 'J')
mdavis30 4:3c22d85a94a8 689 {
tnhnrl 5:7421776f6b08 690 if (LA_auto == false)
tnhnrl 5:7421776f6b08 691 {
tnhnrl 5:7421776f6b08 692 PC.printf("J key pressed. \n");
tnhnrl 5:7421776f6b08 693 PC.printf("%s\n", BLA_object.Keyboard_A());
tnhnrl 5:7421776f6b08 694 }
tnhnrl 5:7421776f6b08 695
tnhnrl 5:7421776f6b08 696 else
tnhnrl 5:7421776f6b08 697 {
tnhnrl 5:7421776f6b08 698 PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
tnhnrl 5:7421776f6b08 699 }
mdavis30 0:381a84fad08b 700 }
mdavis30 4:3c22d85a94a8 701 else if(Key == 'k' or Key == 'K')
mdavis30 4:3c22d85a94a8 702 {
tnhnrl 5:7421776f6b08 703 if (LA_auto == false)
tnhnrl 5:7421776f6b08 704 {
tnhnrl 5:7421776f6b08 705 PC.printf("K key pressed. \n");
tnhnrl 5:7421776f6b08 706 PC.printf("%s\n", BLA_object.Keyboard_D());
tnhnrl 5:7421776f6b08 707 }
tnhnrl 5:7421776f6b08 708
tnhnrl 5:7421776f6b08 709 else
tnhnrl 5:7421776f6b08 710 {
tnhnrl 5:7421776f6b08 711 PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
tnhnrl 5:7421776f6b08 712 }
tnhnrl 5:7421776f6b08 713 }
tnhnrl 5:7421776f6b08 714
tnhnrl 5:7421776f6b08 715 else if (Key == 't')
tnhnrl 5:7421776f6b08 716 {
tnhnrl 5:7421776f6b08 717 PC.printf("VELOCITY?\n%s\n",BLA_object.get_vel());
mdavis30 4:3c22d85a94a8 718 }
mdavis30 4:3c22d85a94a8 719
mdavis30 0:381a84fad08b 720
mdavis30 0:381a84fad08b 721 else
mdavis30 0:381a84fad08b 722 {
mdavis30 0:381a84fad08b 723 PC.printf("\n%c received!\n", Key);
mdavis30 0:381a84fad08b 724 PC.printf("\nDoing nothing.\n");
mdavis30 0:381a84fad08b 725 }
mdavis30 0:381a84fad08b 726
mdavis30 0:381a84fad08b 727 wait_us(100); //for PC readable
mdavis30 3:1257a7d2eb3a 728 //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str()); //get output string
mdavis30 3:1257a7d2eb3a 729 //BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
tnhnrl 5:7421776f6b08 730 }
tnhnrl 5:7421776f6b08 731
tnhnrl 5:7421776f6b08 732 //MC readable
tnhnrl 5:7421776f6b08 733 string MC_readable_string = "";
tnhnrl 5:7421776f6b08 734 MC_readable_string = BLA_object.MC_readable_redux();
tnhnrl 5:7421776f6b08 735 //PC.printf("CHECK_MC_readable:\n%s\n", MC_readable_string);
mdavis30 0:381a84fad08b 736
tnhnrl 5:7421776f6b08 737 if (LA_auto == false)
mdavis30 4:3c22d85a94a8 738 {
tnhnrl 5:7421776f6b08 739 if (!MC_readable_string.empty()) //if this string is empty
tnhnrl 5:7421776f6b08 740 {
tnhnrl 5:7421776f6b08 741 PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller
tnhnrl 5:7421776f6b08 742 }
tnhnrl 5:7421776f6b08 743 else
tnhnrl 5:7421776f6b08 744 {
tnhnrl 5:7421776f6b08 745 ;
tnhnrl 5:7421776f6b08 746 //PC.printf("NOTHING?\n");
tnhnrl 5:7421776f6b08 747 }
mdavis30 4:3c22d85a94a8 748 }
tnhnrl 5:7421776f6b08 749
tnhnrl 5:7421776f6b08 750 //change between automatic and manual mode of linear actuator? Troy: 7/11/2017
tnhnrl 5:7421776f6b08 751
tnhnrl 5:7421776f6b08 752 if (la_cases==0)
tnhnrl 5:7421776f6b08 753 {
tnhnrl 5:7421776f6b08 754 if (debug_mode) //debug mode true
tnhnrl 5:7421776f6b08 755 PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str()); //get output string
tnhnrl 5:7421776f6b08 756 else //debug mode false
tnhnrl 5:7421776f6b08 757 BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
tnhnrl 5:7421776f6b08 758 }
tnhnrl 5:7421776f6b08 759 else if (la_cases==1)
mdavis30 4:3c22d85a94a8 760 {
mdavis30 4:3c22d85a94a8 761
mdavis30 4:3c22d85a94a8 762 while (count_while==0)
mdavis30 4:3c22d85a94a8 763 {
mdavis30 4:3c22d85a94a8 764 PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled
mdavis30 4:3c22d85a94a8 765 PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor
mdavis30 4:3c22d85a94a8 766 wait(1);
mdavis30 4:3c22d85a94a8 767 PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor
mdavis30 4:3c22d85a94a8 768 wait(1);
mdavis30 4:3c22d85a94a8 769 PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n");
mdavis30 4:3c22d85a94a8 770 count_while++;
mdavis30 4:3c22d85a94a8 771 }
mdavis30 4:3c22d85a94a8 772 }
mdavis30 4:3c22d85a94a8 773
mdavis30 0:381a84fad08b 774 if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0))
mdavis30 0:381a84fad08b 775 {
mdavis30 0:381a84fad08b 776 count ++;
mdavis30 0:381a84fad08b 777 //pc().printf("We have a small issue\n");
tnhnrl 5:7421776f6b08 778 //PC.printf("posCon().getOutput() %f\n", posCon().getOutput()); //what is this again?
tnhnrl 5:7421776f6b08 779 //always 1?
tnhnrl 5:7421776f6b08 780 if(count==10)
tnhnrl 5:7421776f6b08 781 {
tnhnrl 5:7421776f6b08 782 //PC.printf("> > > Bad pot issue?\n");
tnhnrl 5:7421776f6b08 783 //hBridge().stop();
tnhnrl 5:7421776f6b08 784 count = 0; //reset counter
tnhnrl 5:7421776f6b08 785 }
mdavis30 0:381a84fad08b 786
mdavis30 0:381a84fad08b 787 }
mdavis30 2:c3cb3ea3c9fa 788 else if ((5.0*ain.read())<1.0)
mdavis30 2:c3cb3ea3c9fa 789 {
tnhnrl 5:7421776f6b08 790 PC.printf("Hit the limit switch??\n");
mdavis30 2:c3cb3ea3c9fa 791 hBridge().stop();
mdavis30 2:c3cb3ea3c9fa 792 }
tnhnrl 5:7421776f6b08 793
tnhnrl 5:7421776f6b08 794
mdavis30 2:c3cb3ea3c9fa 795
tnhnrl 5:7421776f6b08 796 /* buoyancy engine potentiometer string snaps */
mdavis30 0:381a84fad08b 797 else if (pvf().getVelocity() > 100)
mdavis30 0:381a84fad08b 798 {
tnhnrl 5:7421776f6b08 799 PC.printf("DEBUG: String position? %f velocity? %f\n", pvf().getPosition(), pvf().getVelocity()); //DEBUG TROY
mdavis30 0:381a84fad08b 800 //hBridge().stop();
tnhnrl 5:7421776f6b08 801 //PC.printf("PosVelFilter B.E. Velocity: %f\n", pvf().getVelocity());
tnhnrl 5:7421776f6b08 802 PC.printf("********** String broke? *********\n");
mdavis30 4:3c22d85a94a8 803 }
tnhnrl 5:7421776f6b08 804
tnhnrl 5:7421776f6b08 805 if (debug_mode)
tnhnrl 5:7421776f6b08 806 PC.printf("+");
tnhnrl 5:7421776f6b08 807 //PC.printf("DEBUG: End of while loop?\n");
tnhnrl 5:7421776f6b08 808 } //end of while loop
mdavis30 0:381a84fad08b 809 }