FSG
/
7_26_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 System mbed
Fork of
7_21_17_FSG
by 
FSG
main.cpp@12:fa9f84f2967f, 2017-07-28 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jul 28 18:58:07 2017 +0000
- Revision:
- 12:fa9f84f2967f
- Parent:
- 11:b5cc98f154a4
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 | 10:68c3b3f9dc50 | 5 | //new line to get the publish working |
| 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 | |
| mdavis30 | 0:381a84fad08b | 11 | #include "IMU_code.h" //IMU code |
| mdavis30 | 0:381a84fad08b | 12 | |
| mdavis30 | 0:381a84fad08b | 13 | #include "Battery_Linear_Actuator.h" //Battery Linear Actuator code (TROY) (FIX INCLUSION ISSUES, ports) |
| mdavis30 | 0:381a84fad08b | 14 | |
| mdavis30 | 0:381a84fad08b | 15 | Serial PC(USBTX,USBRX); //tx, rx |
| mdavis30 | 0:381a84fad08b | 16 | |
| mdavis30 | 0:381a84fad08b | 17 | extern "C" void mbed_reset(); //utilized to reset the mbed through the serial terminal |
| mdavis30 | 0:381a84fad08b | 18 | |
| mdavis30 | 0:381a84fad08b | 19 | char Key; |
| mdavis30 | 8:9b9431404db7 | 20 | float input_num; |
| mdavis30 | 0:381a84fad08b | 21 | |
| mdavis30 | 0:381a84fad08b | 22 | string IMU_STRING = ""; |
| mdavis30 | 0:381a84fad08b | 23 | |
| mdavis30 | 0:381a84fad08b | 24 | |
| mdavis30 | 0:381a84fad08b | 25 | DigitalOut led1(LED1); |
| mdavis30 | 0:381a84fad08b | 26 | DigitalOut led2(LED2); |
| mdavis30 | 0:381a84fad08b | 27 | DigitalOut led3(LED3); |
| mdavis30 | 0:381a84fad08b | 28 | DigitalOut led4(LED4); |
| mdavis30 | 0:381a84fad08b | 29 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 30 | //Pin for limit switch for buoyancy engine |
| mdavis30 | 2:c3cb3ea3c9fa | 31 | AnalogIn ain(p18); |
| mdavis30 | 0:381a84fad08b | 32 | |
| mdavis30 | 0:381a84fad08b | 33 | /* ************ These tickers work independent of any while loops ********** */ |
| mdavis30 | 0:381a84fad08b | 34 | Ticker IMU_ticker; //ticker for printing IMU //https://developer.mbed.org/handbook/Ticker |
| mdavis30 | 0:381a84fad08b | 35 | Ticker BE_position_ticker; //probably delete soon |
| mdavis30 | 0:381a84fad08b | 36 | Ticker PRESSURE_ticker; |
| mdavis30 | 0:381a84fad08b | 37 | |
| mdavis30 | 0:381a84fad08b | 38 | Ticker BCE_ticker; //new 6/5/17 |
| mdavis30 | 3:1257a7d2eb3a | 39 | Ticker PID_ticker; //new 6/14/17 |
| mdavis30 | 3:1257a7d2eb3a | 40 | Ticker LA_ticker; //new 6/22/17 |
| mdavis30 | 0:381a84fad08b | 41 | /* ************************************************************************* */ |
| mdavis30 | 0:381a84fad08b | 42 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 43 | //Set beginning position for buoyancy and linear actuator |
| mdavis30 | 6:ce2cf7f4d7d5 | 44 | float positionCmd = 190.0; |
| mdavis30 | 4:3c22d85a94a8 | 45 | float pi = 3.14159265359; |
| mdavis30 | 6:ce2cf7f4d7d5 | 46 | float la_setPoint = 0.00; //the IMU pitch angle we want (setpoint) |
| mdavis30 | 4:3c22d85a94a8 | 47 | |
| mdavis30 | 3:1257a7d2eb3a | 48 | /* PID LOOP STUFF */ |
| mdavis30 | 3:1257a7d2eb3a | 49 | float la_P_gain = 1.0; |
| mdavis30 | 3:1257a7d2eb3a | 50 | float la_I_gain = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 51 | float la_D_gain = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 52 | /* PID LOOP STUFF */ |
| mdavis30 | 3:1257a7d2eb3a | 53 | |
| mdavis30 | 8:9b9431404db7 | 54 | int count_while = 0; |
| mdavis30 | 8:9b9431404db7 | 55 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 56 | float IMU_pitch_angle; |
| mdavis30 | 6:ce2cf7f4d7d5 | 57 | double double_actual_position = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 58 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 59 | string MC_readable_string = ""; |
| tnhnrl | 5:7421776f6b08 | 60 | |
| mdavis30 | 8:9b9431404db7 | 61 | int IMU_count = 0; |
| mdavis30 | 8:9b9431404db7 | 62 | int Pr_count = 0; |
| mdavis30 | 8:9b9431404db7 | 63 | int BCE_count = 0; |
| mdavis30 | 8:9b9431404db7 | 64 | |
| mdavis30 | 12:fa9f84f2967f | 65 | AnalogIn pressure_analog_in(A0); |
| mdavis30 | 12:fa9f84f2967f | 66 | |
| mdavis30 | 0:381a84fad08b | 67 | void IMU_ticking() |
| mdavis30 | 0:381a84fad08b | 68 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 69 | //led1 = !led1; //flash the IMU LED |
| mdavis30 | 6:ce2cf7f4d7d5 | 70 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 71 | //PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it |
| mdavis30 | 6:ce2cf7f4d7d5 | 72 | PC.printf("%s\n", IMU_STRING.c_str()); |
| mdavis30 | 0:381a84fad08b | 73 | } |
| mdavis30 | 0:381a84fad08b | 74 | |
| mdavis30 | 0:381a84fad08b | 75 | void PRESSURE_ticking() |
| mdavis30 | 0:381a84fad08b | 76 | { |
| mdavis30 | 12:fa9f84f2967f | 77 | PC.printf("Pressure sensor (PSI)\nPRESS_PSI: %3.3f\n", 3.3*1.503*10 * pressure_analog_in.read()); //read the analog pin //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 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 83 | 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 | 84 | } |
| mdavis30 | 0:381a84fad08b | 85 | |
| mdavis30 | 8:9b9431404db7 | 86 | |
| mdavis30 | 0:381a84fad08b | 87 | int main() |
| mdavis30 | 0:381a84fad08b | 88 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 89 | PC.baud(9600); //mbed to PC serial connection speed |
| mdavis30 | 6:ce2cf7f4d7d5 | 90 | hBridge().stop(); //Stop the buoyancy engine from moving |
| mdavis30 | 3:1257a7d2eb3a | 91 | systemTime().start(); //start the timer, needed for PID loop |
| mdavis30 | 3:1257a7d2eb3a | 92 | |
| mdavis30 | 0:381a84fad08b | 93 | //setup and start the adc. This runs on a fixed interval and is interrupt driven |
| mdavis30 | 0:381a84fad08b | 94 | adc().initialize(); |
| mdavis30 | 0:381a84fad08b | 95 | adc().start(); |
| mdavis30 | 0:381a84fad08b | 96 | |
| mdavis30 | 0:381a84fad08b | 97 | //Initialize the position velocity filter. This will consume a couple of seconds for |
| mdavis30 | 0:381a84fad08b | 98 | //the filter to converge |
| mdavis30 | 0:381a84fad08b | 99 | pvf().init(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 100 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 101 | |
| mdavis30 | 0:381a84fad08b | 102 | float P = 0.10; |
| mdavis30 | 0:381a84fad08b | 103 | float I = 0.00; |
| mdavis30 | 0:381a84fad08b | 104 | float D = 0.00; |
| mdavis30 | 0:381a84fad08b | 105 | float count = 0.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 106 | float positionCmd_cm; |
| mdavis30 | 0:381a84fad08b | 107 | |
| mdavis30 | 4:3c22d85a94a8 | 108 | float la_step = 1.0; |
| mdavis30 | 4:3c22d85a94a8 | 109 | float la_setPoint_temp = 0.0; |
| mdavis30 | 4:3c22d85a94a8 | 110 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 111 | bool BCE_auto = true; |
| tnhnrl | 5:7421776f6b08 | 112 | bool LA_auto = false; |
| mdavis30 | 4:3c22d85a94a8 | 113 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 114 | float bce_auto_step = 0.1; |
| mdavis30 | 6:ce2cf7f4d7d5 | 115 | float volume_bce = 1.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 116 | int bce_man_step = 50; |
| mdavis30 | 8:9b9431404db7 | 117 | |
| mdavis30 | 8:9b9431404db7 | 118 | float ticker_step_size = 1.0; |
| mdavis30 | 0:381a84fad08b | 119 | |
| mdavis30 | 0:381a84fad08b | 120 | //set the intial gains for the position controller |
| mdavis30 | 8:9b9431404db7 | 121 | posCon().setPgain(P); |
| mdavis30 | 0:381a84fad08b | 122 | posCon().setIgain(I); |
| mdavis30 | 0:381a84fad08b | 123 | posCon().setDgain(D); |
| mdavis30 | 6:ce2cf7f4d7d5 | 124 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 125 | hBridge().reset(); |
| mdavis30 | 8:9b9431404db7 | 126 | //hBridge().run(motor_cmd); |
| mdavis30 | 8:9b9431404db7 | 127 | |
| mdavis30 | 0:381a84fad08b | 128 | /* *************************** LED *************************** */ |
| mdavis30 | 0:381a84fad08b | 129 | led1 = 1; //initial values |
| mdavis30 | 0:381a84fad08b | 130 | led2 = 1; |
| mdavis30 | 6:ce2cf7f4d7d5 | 131 | led3 = 1; |
| mdavis30 | 0:381a84fad08b | 132 | led4 = 1; |
| mdavis30 | 0:381a84fad08b | 133 | /* *************************** LED *************************** */ |
| mdavis30 | 6:ce2cf7f4d7d5 | 134 | |
| mdavis30 | 8:9b9431404db7 | 135 | IMU_code IMU_object; |
| mdavis30 | 0:381a84fad08b | 136 | |
| mdavis30 | 0:381a84fad08b | 137 | while(1) |
| mdavis30 | 0:381a84fad08b | 138 | { |
| mdavis30 | 0:381a84fad08b | 139 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 140 | IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop |
| mdavis30 | 6:ce2cf7f4d7d5 | 141 | IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly? |
| mdavis30 | 6:ce2cf7f4d7d5 | 142 | //PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_yaw_angle, la_setPoint); |
| mdavis30 | 0:381a84fad08b | 143 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 144 | |
| mdavis30 | 0:381a84fad08b | 145 | /* Buoyancy Engine */ |
| mdavis30 | 0:381a84fad08b | 146 | // update the position velocity filter |
| mdavis30 | 0:381a84fad08b | 147 | pvf().update(); |
| mdavis30 | 0:381a84fad08b | 148 | |
| mdavis30 | 0:381a84fad08b | 149 | //update the controller with the current numbers in the position guesser |
| mdavis30 | 0:381a84fad08b | 150 | posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ; |
| mdavis30 | 0:381a84fad08b | 151 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 0:381a84fad08b | 152 | |
| mdavis30 | 0:381a84fad08b | 153 | /* Buoyancy Engine */ |
| mdavis30 | 0:381a84fad08b | 154 | |
| mdavis30 | 0:381a84fad08b | 155 | //FOR DEBUGGING |
| mdavis30 | 0:381a84fad08b | 156 | //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 | 157 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 158 | //PC.printf("WHILE LOOP\n"); //DEBUG |
| mdavis30 | 0:381a84fad08b | 159 | if (PC.readable()) |
| mdavis30 | 0:381a84fad08b | 160 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 161 | //led4 != led4; |
| mdavis30 | 6:ce2cf7f4d7d5 | 162 | //PC.printf("DEBUG: PC IS READABLE\n"); //DEBUG |
| mdavis30 | 0:381a84fad08b | 163 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 164 | Key=PC.getc(); |
| mdavis30 | 4:3c22d85a94a8 | 165 | //Universal MBED Controls |
| mdavis30 | 6:ce2cf7f4d7d5 | 166 | if(Key=='!') //RESET THE MBED |
| mdavis30 | 0:381a84fad08b | 167 | { |
| mdavis30 | 4:3c22d85a94a8 | 168 | PC.printf("MBED RESET KEY (!) PRESSED\n"); |
| mdavis30 | 4:3c22d85a94a8 | 169 | mbed_reset(); //reset the mbed! |
| mdavis30 | 12:fa9f84f2967f | 170 | } |
| mdavis30 | 8:9b9431404db7 | 171 | else if (Key == '3' or Key == '#') |
| mdavis30 | 8:9b9431404db7 | 172 | { |
| mdavis30 | 8:9b9431404db7 | 173 | PC.printf("Ticker Sensor Step Size\n"); |
| mdavis30 | 8:9b9431404db7 | 174 | if (ticker_step_size == 1.0) |
| mdavis30 | 8:9b9431404db7 | 175 | { |
| mdavis30 | 8:9b9431404db7 | 176 | ticker_step_size = 5.0; |
| mdavis30 | 8:9b9431404db7 | 177 | } |
| mdavis30 | 8:9b9431404db7 | 178 | else if (ticker_step_size == 5.0) |
| mdavis30 | 8:9b9431404db7 | 179 | { |
| mdavis30 | 8:9b9431404db7 | 180 | ticker_step_size = 10.0; |
| mdavis30 | 8:9b9431404db7 | 181 | } |
| mdavis30 | 8:9b9431404db7 | 182 | else if (ticker_step_size == 10.0) |
| mdavis30 | 8:9b9431404db7 | 183 | { |
| mdavis30 | 8:9b9431404db7 | 184 | ticker_step_size = 20.0; |
| mdavis30 | 8:9b9431404db7 | 185 | } |
| mdavis30 | 8:9b9431404db7 | 186 | else if (ticker_step_size == 20.0) |
| mdavis30 | 8:9b9431404db7 | 187 | { |
| mdavis30 | 8:9b9431404db7 | 188 | ticker_step_size = 1.0; |
| mdavis30 | 8:9b9431404db7 | 189 | } |
| mdavis30 | 8:9b9431404db7 | 190 | PC.printf("Ticker Step Size is %f\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 191 | } |
| mdavis30 | 8:9b9431404db7 | 192 | else if (Key =='4' or Key == '$') |
| mdavis30 | 8:9b9431404db7 | 193 | { |
| mdavis30 | 8:9b9431404db7 | 194 | if (IMU_count == 0) |
| mdavis30 | 8:9b9431404db7 | 195 | { |
| mdavis30 | 8:9b9431404db7 | 196 | PC.printf("IMU turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 197 | IMU_ticker.attach(&IMU_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 198 | IMU_count = 1; |
| mdavis30 | 8:9b9431404db7 | 199 | } |
| mdavis30 | 8:9b9431404db7 | 200 | else if (IMU_count == 1) |
| mdavis30 | 8:9b9431404db7 | 201 | { |
| mdavis30 | 8:9b9431404db7 | 202 | PC.printf("IMU turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 203 | IMU_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 204 | IMU_count = 0; |
| mdavis30 | 8:9b9431404db7 | 205 | } |
| mdavis30 | 8:9b9431404db7 | 206 | |
| mdavis30 | 8:9b9431404db7 | 207 | } |
| mdavis30 | 8:9b9431404db7 | 208 | else if (Key == '5' or Key == '%') |
| mdavis30 | 8:9b9431404db7 | 209 | { |
| mdavis30 | 8:9b9431404db7 | 210 | if (Pr_count == 0) |
| mdavis30 | 8:9b9431404db7 | 211 | { |
| mdavis30 | 8:9b9431404db7 | 212 | PC.printf("Pressure turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 213 | PRESSURE_ticker.attach(&PRESSURE_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 214 | Pr_count = 1; |
| mdavis30 | 8:9b9431404db7 | 215 | } |
| mdavis30 | 8:9b9431404db7 | 216 | else if (Pr_count == 1) |
| mdavis30 | 8:9b9431404db7 | 217 | { |
| mdavis30 | 8:9b9431404db7 | 218 | PC.printf("Pressure turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 219 | PRESSURE_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 220 | Pr_count = 0; |
| mdavis30 | 8:9b9431404db7 | 221 | } |
| mdavis30 | 8:9b9431404db7 | 222 | } |
| mdavis30 | 8:9b9431404db7 | 223 | else if (Key == '6' or Key == '^') |
| mdavis30 | 8:9b9431404db7 | 224 | { |
| mdavis30 | 8:9b9431404db7 | 225 | if (BCE_count == 0) |
| mdavis30 | 8:9b9431404db7 | 226 | { |
| mdavis30 | 8:9b9431404db7 | 227 | PC.printf("BCE turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 228 | BCE_ticker.attach(&BCE_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 229 | BCE_count = 1; |
| mdavis30 | 8:9b9431404db7 | 230 | } |
| mdavis30 | 8:9b9431404db7 | 231 | else if (BCE_count == 1) |
| mdavis30 | 8:9b9431404db7 | 232 | { |
| mdavis30 | 8:9b9431404db7 | 233 | PC.printf("BCE turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 234 | BCE_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 235 | BCE_count = 0; |
| mdavis30 | 8:9b9431404db7 | 236 | } |
| mdavis30 | 8:9b9431404db7 | 237 | } |
| mdavis30 | 4:3c22d85a94a8 | 238 | //Buoyancy Engine Controls |
| mdavis30 | 4:3c22d85a94a8 | 239 | else if (Key == ',' or Key == '<') |
| mdavis30 | 4:3c22d85a94a8 | 240 | { |
| mdavis30 | 4:3c22d85a94a8 | 241 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 242 | { |
| mdavis30 | 4:3c22d85a94a8 | 243 | PC.printf("BCE: Now in Automatic Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 244 | BCE_auto = true; |
| mdavis30 | 4:3c22d85a94a8 | 245 | } |
| mdavis30 | 4:3c22d85a94a8 | 246 | else |
| mdavis30 | 4:3c22d85a94a8 | 247 | { |
| tnhnrl | 5:7421776f6b08 | 248 | PC.printf("BCE: Still in Automatic Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 249 | } |
| mdavis30 | 4:3c22d85a94a8 | 250 | } |
| mdavis30 | 4:3c22d85a94a8 | 251 | else if (Key == '.' or Key == '>') |
| mdavis30 | 4:3c22d85a94a8 | 252 | { |
| mdavis30 | 4:3c22d85a94a8 | 253 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 254 | { |
| mdavis30 | 4:3c22d85a94a8 | 255 | PC.printf("BCE: Now in Manual Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 256 | BCE_auto = false; |
| mdavis30 | 4:3c22d85a94a8 | 257 | } |
| mdavis30 | 4:3c22d85a94a8 | 258 | else |
| mdavis30 | 4:3c22d85a94a8 | 259 | { |
| tnhnrl | 5:7421776f6b08 | 260 | PC.printf("BCE: Still in Manual Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 261 | } |
| mdavis30 | 4:3c22d85a94a8 | 262 | } |
| mdavis30 | 4:3c22d85a94a8 | 263 | //BCE Automatic Controls |
| mdavis30 | 6:ce2cf7f4d7d5 | 264 | else if(Key =='d' or Key == 'D') |
| mdavis30 | 4:3c22d85a94a8 | 265 | { |
| mdavis30 | 4:3c22d85a94a8 | 266 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 267 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 268 | volume_bce -= bce_auto_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 269 | PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 270 | } |
| mdavis30 | 4:3c22d85a94a8 | 271 | else |
| mdavis30 | 4:3c22d85a94a8 | 272 | { |
| mdavis30 | 4:3c22d85a94a8 | 273 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 274 | } |
| mdavis30 | 4:3c22d85a94a8 | 275 | } |
| mdavis30 | 4:3c22d85a94a8 | 276 | else if(Key == 'f' or Key == 'F') |
| mdavis30 | 4:3c22d85a94a8 | 277 | { |
| mdavis30 | 4:3c22d85a94a8 | 278 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 279 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 280 | volume_bce += bce_auto_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 281 | PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 282 | } |
| mdavis30 | 4:3c22d85a94a8 | 283 | else |
| mdavis30 | 4:3c22d85a94a8 | 284 | { |
| mdavis30 | 4:3c22d85a94a8 | 285 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 286 | } |
| mdavis30 | 4:3c22d85a94a8 | 287 | } |
| mdavis30 | 4:3c22d85a94a8 | 288 | else if(Key == 'r' or Key == 'R') |
| mdavis30 | 4:3c22d85a94a8 | 289 | { |
| mdavis30 | 4:3c22d85a94a8 | 290 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 291 | { |
| mdavis30 | 4:3c22d85a94a8 | 292 | PC.printf("\nR received!\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 293 | positionCmd_cm=(1000/(16*pi))*volume_bce; |
| mdavis30 | 6:ce2cf7f4d7d5 | 294 | positionCmd = positionCmd_cm*10; |
| mdavis30 | 6:ce2cf7f4d7d5 | 295 | //positionCmd= positionCmd_temp*0.000000001; |
| mdavis30 | 6:ce2cf7f4d7d5 | 296 | //PC.printf("BCE engine going to position: %3.2f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 297 | PC.printf("\nBASETP: %3.0f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 298 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 299 | //posCon().setPgain(P); |
| mdavis30 | 6:ce2cf7f4d7d5 | 300 | //posCon().setIgain(I); |
| mdavis30 | 6:ce2cf7f4d7d5 | 301 | //posCon().setDgain(D); |
| mdavis30 | 6:ce2cf7f4d7d5 | 302 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 6:ce2cf7f4d7d5 | 303 | |
| mdavis30 | 4:3c22d85a94a8 | 304 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 305 | count = 0; |
| mdavis30 | 4:3c22d85a94a8 | 306 | } |
| mdavis30 | 4:3c22d85a94a8 | 307 | else |
| mdavis30 | 4:3c22d85a94a8 | 308 | { |
| mdavis30 | 4:3c22d85a94a8 | 309 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 310 | } |
| mdavis30 | 4:3c22d85a94a8 | 311 | } |
| mdavis30 | 4:3c22d85a94a8 | 312 | //BCE Manual Controls |
| mdavis30 | 4:3c22d85a94a8 | 313 | else if (Key == '2' or Key == '@') |
| mdavis30 | 4:3c22d85a94a8 | 314 | { |
| mdavis30 | 4:3c22d85a94a8 | 315 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 316 | { |
| mdavis30 | 4:3c22d85a94a8 | 317 | PC.printf("BCE Manual Step Size Change\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 318 | if (bce_man_step == 50) |
| mdavis30 | 4:3c22d85a94a8 | 319 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 320 | bce_man_step = 25; |
| mdavis30 | 4:3c22d85a94a8 | 321 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 322 | else if (bce_man_step == 25) |
| mdavis30 | 4:3c22d85a94a8 | 323 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 324 | bce_man_step = 10; |
| mdavis30 | 4:3c22d85a94a8 | 325 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 326 | else if (bce_man_step == 10) |
| mdavis30 | 4:3c22d85a94a8 | 327 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 328 | bce_man_step = 1; |
| mdavis30 | 4:3c22d85a94a8 | 329 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 330 | else if (bce_man_step == 1) |
| mdavis30 | 4:3c22d85a94a8 | 331 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 332 | bce_man_step = 50; |
| mdavis30 | 4:3c22d85a94a8 | 333 | } |
| mdavis30 | 4:3c22d85a94a8 | 334 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 335 | PC.printf("BCE Manual Step Size Now %d\n", bce_man_step); |
| mdavis30 | 4:3c22d85a94a8 | 336 | } |
| mdavis30 | 4:3c22d85a94a8 | 337 | else |
| mdavis30 | 4:3c22d85a94a8 | 338 | { |
| mdavis30 | 4:3c22d85a94a8 | 339 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 340 | } |
| mdavis30 | 4:3c22d85a94a8 | 341 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 342 | else if (Key == 'z' or Key =='Z') |
| mdavis30 | 6:ce2cf7f4d7d5 | 343 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 344 | if (BCE_auto == false and positionCmd < 395) |
| mdavis30 | 6:ce2cf7f4d7d5 | 345 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 346 | //increment the duty cycle |
| mdavis30 | 6:ce2cf7f4d7d5 | 347 | positionCmd -= bce_man_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 348 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 6:ce2cf7f4d7d5 | 349 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 350 | else if (positionCmd > 395) |
| mdavis30 | 6:ce2cf7f4d7d5 | 351 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 352 | PC.printf("ERROR: Cannot move past 395 mm\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 353 | positionCmd = 370; |
| mdavis30 | 6:ce2cf7f4d7d5 | 354 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 355 | else |
| mdavis30 | 6:ce2cf7f4d7d5 | 356 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 357 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 358 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 359 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 360 | else if (Key == 'x' or Key == 'X') |
| mdavis30 | 6:ce2cf7f4d7d5 | 361 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 362 | if (BCE_auto == false and positionCmd > -25) |
| mdavis30 | 6:ce2cf7f4d7d5 | 363 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 364 | //decrement the duty cycle |
| mdavis30 | 6:ce2cf7f4d7d5 | 365 | positionCmd += bce_man_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 366 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 6:ce2cf7f4d7d5 | 367 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 368 | else if (positionCmd < -25) |
| mdavis30 | 6:ce2cf7f4d7d5 | 369 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 370 | PC.printf("ERROR: Cannot move past -5 mm\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 371 | positionCmd = -5; |
| mdavis30 | 6:ce2cf7f4d7d5 | 372 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 373 | else |
| mdavis30 | 6:ce2cf7f4d7d5 | 374 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 375 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 376 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 377 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 378 | else if(Key=='w' or Key =='W') |
| mdavis30 | 4:3c22d85a94a8 | 379 | { |
| mdavis30 | 4:3c22d85a94a8 | 380 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 381 | { |
| mdavis30 | 4:3c22d85a94a8 | 382 | PC.printf("\nW received!\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 383 | PC.printf("BASETP: %3.0f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 384 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 4:3c22d85a94a8 | 385 | //posCon().setPgain(P); |
| mdavis30 | 4:3c22d85a94a8 | 386 | //posCon().setIgain(I); |
| mdavis30 | 4:3c22d85a94a8 | 387 | //posCon().setDgain(D); |
| mdavis30 | 4:3c22d85a94a8 | 388 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 4:3c22d85a94a8 | 389 | |
| mdavis30 | 4:3c22d85a94a8 | 390 | hBridge().reset(); |
| mdavis30 | 4:3c22d85a94a8 | 391 | |
| mdavis30 | 4:3c22d85a94a8 | 392 | count = 0; |
| mdavis30 | 4:3c22d85a94a8 | 393 | |
| mdavis30 | 4:3c22d85a94a8 | 394 | } |
| mdavis30 | 4:3c22d85a94a8 | 395 | else |
| mdavis30 | 4:3c22d85a94a8 | 396 | { |
| mdavis30 | 4:3c22d85a94a8 | 397 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 398 | } |
| mdavis30 | 4:3c22d85a94a8 | 399 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 400 | else if (Key == 'l' or Key == 'L') |
| mdavis30 | 8:9b9431404db7 | 401 | PC.printf("DEBUG: String position? %f set position? %f velocity? %f (BCE active: %f)\n", pvf().getPosition(), positionCmd, pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY |
| mdavis30 | 4:3c22d85a94a8 | 402 | |
| mdavis30 | 4:3c22d85a94a8 | 403 | //Linear Actuator Controls |
| mdavis30 | 4:3c22d85a94a8 | 404 | else if(Key == 'c' or Key == 'C') |
| mdavis30 | 4:3c22d85a94a8 | 405 | { |
| mdavis30 | 4:3c22d85a94a8 | 406 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 407 | { |
| mdavis30 | 4:3c22d85a94a8 | 408 | PC.printf("ERROR: LA already in auto mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 409 | } |
| mdavis30 | 4:3c22d85a94a8 | 410 | else |
| mdavis30 | 4:3c22d85a94a8 | 411 | { |
| mdavis30 | 4:3c22d85a94a8 | 412 | LA_auto = true; |
| mdavis30 | 6:ce2cf7f4d7d5 | 413 | PC.printf("```````````Now in IMU Controlled Mode```````````````\n"); |
| mdavis30 | 4:3c22d85a94a8 | 414 | count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 415 | } |
| mdavis30 | 4:3c22d85a94a8 | 416 | } |
| mdavis30 | 4:3c22d85a94a8 | 417 | else if (Key == 'v' or Key == 'V') |
| mdavis30 | 4:3c22d85a94a8 | 418 | { |
| mdavis30 | 4:3c22d85a94a8 | 419 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 420 | { |
| mdavis30 | 4:3c22d85a94a8 | 421 | LA_auto = false; |
| mdavis30 | 6:ce2cf7f4d7d5 | 422 | //go from imu controlled to manual |
| mdavis30 | 6:ce2cf7f4d7d5 | 423 | PC.printf("```````````Now in Manual Mode````````````````````\n"); |
| mdavis30 | 4:3c22d85a94a8 | 424 | count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 425 | } |
| mdavis30 | 4:3c22d85a94a8 | 426 | else |
| mdavis30 | 4:3c22d85a94a8 | 427 | { |
| mdavis30 | 4:3c22d85a94a8 | 428 | PC.printf("ERROR: LA already in manual mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 429 | } |
| mdavis30 | 4:3c22d85a94a8 | 430 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 431 | |
| tnhnrl | 5:7421776f6b08 | 432 | |
| tnhnrl | 5:7421776f6b08 | 433 | else |
| tnhnrl | 5:7421776f6b08 | 434 | { |
| tnhnrl | 5:7421776f6b08 | 435 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 436 | } |
| mdavis30 | 4:3c22d85a94a8 | 437 | } |
| mdavis30 | 0:381a84fad08b | 438 | else |
| mdavis30 | 0:381a84fad08b | 439 | { |
| mdavis30 | 0:381a84fad08b | 440 | PC.printf("\n%c received!\n", Key); |
| mdavis30 | 0:381a84fad08b | 441 | PC.printf("\nDoing nothing.\n"); |
| mdavis30 | 0:381a84fad08b | 442 | } |
| mdavis30 | 0:381a84fad08b | 443 | |
| mdavis30 | 0:381a84fad08b | 444 | wait_us(100); //for PC readable |
| mdavis30 | 6:ce2cf7f4d7d5 | 445 | //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_yaw_angle, la_P_gain, la_I_gain, la_D_gain).c_str()); //get output string |
| mdavis30 | 6:ce2cf7f4d7d5 | 446 | //BLA_object.PID_velocity_control(la_setPoint, IMU_yaw_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| tnhnrl | 5:7421776f6b08 | 447 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 448 | //end of PC readBLE |
| mdavis30 | 6:ce2cf7f4d7d5 | 449 | |
| tnhnrl | 5:7421776f6b08 | 450 | //PC.printf("CHECK_MC_readable:\n%s\n", MC_readable_string); |
| mdavis30 | 0:381a84fad08b | 451 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 452 | /* if (LA_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 453 | { |
| tnhnrl | 5:7421776f6b08 | 454 | if (!MC_readable_string.empty()) //if this string is empty |
| tnhnrl | 5:7421776f6b08 | 455 | { |
| tnhnrl | 5:7421776f6b08 | 456 | PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller |
| tnhnrl | 5:7421776f6b08 | 457 | } |
| tnhnrl | 5:7421776f6b08 | 458 | else |
| tnhnrl | 5:7421776f6b08 | 459 | { |
| tnhnrl | 5:7421776f6b08 | 460 | ; |
| tnhnrl | 5:7421776f6b08 | 461 | //PC.printf("NOTHING?\n"); |
| tnhnrl | 5:7421776f6b08 | 462 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 463 | }*/ |
| mdavis30 | 6:ce2cf7f4d7d5 | 464 | if (LA_auto==true) |
| tnhnrl | 5:7421776f6b08 | 465 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 466 | //PC.printf("LA_auto true\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 467 | //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 | 12:fa9f84f2967f | 468 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 469 | //wait_us(100); //for PC readable (0.1 ms) |
| tnhnrl | 5:7421776f6b08 | 470 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 471 | else if (LA_auto == false) |
| mdavis30 | 6:ce2cf7f4d7d5 | 472 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 473 | //PC.printf("LA_auto false\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 474 | // if (!MC_readable_string.empty()) //if this string is empty |
| mdavis30 | 6:ce2cf7f4d7d5 | 475 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 476 | // PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller |
| mdavis30 | 6:ce2cf7f4d7d5 | 477 | // } |
| mdavis30 | 6:ce2cf7f4d7d5 | 478 | // else |
| mdavis30 | 6:ce2cf7f4d7d5 | 479 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 480 | // ; |
| mdavis30 | 6:ce2cf7f4d7d5 | 481 | // //PC.printf("NOTHING?\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 482 | // } |
| mdavis30 | 4:3c22d85a94a8 | 483 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 484 | // while (count_while==0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 485 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 486 | //// PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled |
| mdavis30 | 6:ce2cf7f4d7d5 | 487 | //// PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 488 | //// wait(1); |
| mdavis30 | 6:ce2cf7f4d7d5 | 489 | //// PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 490 | //// wait(1); |
| mdavis30 | 6:ce2cf7f4d7d5 | 491 | //// PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 492 | //// count_while++; |
| mdavis30 | 6:ce2cf7f4d7d5 | 493 | // |
| mdavis30 | 6:ce2cf7f4d7d5 | 494 | // BLA_object.Keyboard_E(); //turn on motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 495 | // BLA_object.velocity_only(0); //set the velocity to zero just in case |
| mdavis30 | 6:ce2cf7f4d7d5 | 496 | // PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 497 | // count_while++; |
| mdavis30 | 6:ce2cf7f4d7d5 | 498 | // } |
| mdavis30 | 4:3c22d85a94a8 | 499 | } |
| mdavis30 | 4:3c22d85a94a8 | 500 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 501 | |
| mdavis30 | 0:381a84fad08b | 502 | if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0)) |
| mdavis30 | 0:381a84fad08b | 503 | { |
| mdavis30 | 0:381a84fad08b | 504 | count ++; |
| mdavis30 | 0:381a84fad08b | 505 | //pc().printf("We have a small issue\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 506 | if(count==10) |
| mdavis30 | 6:ce2cf7f4d7d5 | 507 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 508 | pc().printf("Bad pot issue\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 509 | //hBridge().stop(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 510 | count = 0; |
| mdavis30 | 7:10d7fbac30ea | 511 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 512 | } |
| mdavis30 | 0:381a84fad08b | 513 | |
| mdavis30 | 0:381a84fad08b | 514 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 515 | else if ((5.0*ain.read())<1.0) |
| mdavis30 | 2:c3cb3ea3c9fa | 516 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 517 | pc().printf("Hit the limit switch??\n"); |
| mdavis30 | 2:c3cb3ea3c9fa | 518 | hBridge().stop(); |
| mdavis30 | 7:10d7fbac30ea | 519 | wait(1); |
| mdavis30 | 7:10d7fbac30ea | 520 | hBridge().reset(); |
| mdavis30 | 7:10d7fbac30ea | 521 | positionCmd= 375; |
| mdavis30 | 7:10d7fbac30ea | 522 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 7:10d7fbac30ea | 523 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 8:9b9431404db7 | 524 | wait(2); |
| mdavis30 | 2:c3cb3ea3c9fa | 525 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 526 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 527 | //string snaps |
| mdavis30 | 0:381a84fad08b | 528 | else if (pvf().getVelocity() > 100) |
| mdavis30 | 0:381a84fad08b | 529 | { |
| tnhnrl | 5:7421776f6b08 | 530 | PC.printf("DEBUG: String position? %f velocity? %f\n", pvf().getPosition(), pvf().getVelocity()); //DEBUG TROY |
| mdavis30 | 0:381a84fad08b | 531 | //hBridge().stop(); |
| tnhnrl | 5:7421776f6b08 | 532 | //PC.printf("PosVelFilter B.E. Velocity: %f\n", pvf().getVelocity()); |
| mdavis30 | 6:ce2cf7f4d7d5 | 533 | PC.printf("********** String broke? *********\n"); |
| mdavis30 | 4:3c22d85a94a8 | 534 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 535 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 536 | } |
| mdavis30 | 12:fa9f84f2967f | 537 |