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
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?
User | Revision | Line number | New 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 | } |