FSG
/
7_21_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_20_17_FSG_
by 
FSG
main.cpp@8:9b9431404db7, 2017-07-25 (annotated)
- Committer:
- mdavis30
- Date:
- Tue Jul 25 17:41:34 2017 +0000
- Revision:
- 8:9b9431404db7
- Parent:
- 7:10d7fbac30ea
- Child:
- 9:2dcfd6ce61ea
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 | |
| 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 | 0:381a84fad08b | 65 | void IMU_ticking() |
| mdavis30 | 0:381a84fad08b | 66 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 67 | //led1 = !led1; //flash the IMU LED |
| mdavis30 | 6:ce2cf7f4d7d5 | 68 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 69 | //PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it |
| mdavis30 | 6:ce2cf7f4d7d5 | 70 | PC.printf("%s\n", IMU_STRING.c_str()); |
| mdavis30 | 0:381a84fad08b | 71 | } |
| mdavis30 | 0:381a84fad08b | 72 | |
| mdavis30 | 0:381a84fad08b | 73 | void PRESSURE_ticking() |
| mdavis30 | 0:381a84fad08b | 74 | { |
| mdavis30 | 8:9b9431404db7 | 75 | PC.printf("pressure: %f psi \n", (0.00122*(adc().ch1_filt)*9.9542)-0.0845); //read the analog pin |
| mdavis30 | 3:1257a7d2eb3a | 76 | //this voltage has been checked and scaled properly (6/28/2017) |
| mdavis30 | 0:381a84fad08b | 77 | } |
| mdavis30 | 0:381a84fad08b | 78 | |
| mdavis30 | 0:381a84fad08b | 79 | void BCE_ticking() //new 6/5/17 |
| mdavis30 | 0:381a84fad08b | 80 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 81 | 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 | 82 | } |
| mdavis30 | 0:381a84fad08b | 83 | |
| mdavis30 | 8:9b9431404db7 | 84 | |
| mdavis30 | 0:381a84fad08b | 85 | int main() |
| mdavis30 | 0:381a84fad08b | 86 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 87 | PC.baud(9600); //mbed to PC serial connection speed |
| mdavis30 | 6:ce2cf7f4d7d5 | 88 | hBridge().stop(); //Stop the buoyancy engine from moving |
| mdavis30 | 3:1257a7d2eb3a | 89 | systemTime().start(); //start the timer, needed for PID loop |
| mdavis30 | 3:1257a7d2eb3a | 90 | |
| mdavis30 | 3:1257a7d2eb3a | 91 | /* **************** Linear Actuator MOTOR CONTROLLER **************** */ |
| mdavis30 | 3:1257a7d2eb3a | 92 | Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class |
| mdavis30 | 3:1257a7d2eb3a | 93 | |
| mdavis30 | 3:1257a7d2eb3a | 94 | PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled |
| mdavis30 | 3:1257a7d2eb3a | 95 | |
| mdavis30 | 3:1257a7d2eb3a | 96 | PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor |
| mdavis30 | 3:1257a7d2eb3a | 97 | wait(1); |
| mdavis30 | 3:1257a7d2eb3a | 98 | PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor |
| mdavis30 | 3:1257a7d2eb3a | 99 | wait(1); |
| mdavis30 | 3:1257a7d2eb3a | 100 | |
| mdavis30 | 0:381a84fad08b | 101 | //setup and start the adc. This runs on a fixed interval and is interrupt driven |
| mdavis30 | 0:381a84fad08b | 102 | adc().initialize(); |
| mdavis30 | 0:381a84fad08b | 103 | adc().start(); |
| mdavis30 | 0:381a84fad08b | 104 | |
| mdavis30 | 0:381a84fad08b | 105 | //Initialize the position velocity filter. This will consume a couple of seconds for |
| mdavis30 | 0:381a84fad08b | 106 | //the filter to converge |
| mdavis30 | 0:381a84fad08b | 107 | pvf().init(); |
| mdavis30 | 0:381a84fad08b | 108 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 109 | //NEW 7/21 Homing the motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 110 | PC.printf("### homing the device ###"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 111 | BLA_object.Keyboard_H(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 112 | wait(10); //for debugging |
| mdavis30 | 6:ce2cf7f4d7d5 | 113 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 114 | |
| mdavis30 | 0:381a84fad08b | 115 | float P = 0.10; |
| mdavis30 | 0:381a84fad08b | 116 | float I = 0.00; |
| mdavis30 | 0:381a84fad08b | 117 | float D = 0.00; |
| mdavis30 | 0:381a84fad08b | 118 | float count = 0.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 119 | float positionCmd_cm; |
| mdavis30 | 0:381a84fad08b | 120 | |
| mdavis30 | 4:3c22d85a94a8 | 121 | float la_step = 1.0; |
| mdavis30 | 4:3c22d85a94a8 | 122 | float la_setPoint_temp = 0.0; |
| mdavis30 | 4:3c22d85a94a8 | 123 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 124 | bool BCE_auto = true; |
| tnhnrl | 5:7421776f6b08 | 125 | bool LA_auto = false; |
| mdavis30 | 4:3c22d85a94a8 | 126 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 127 | float bce_auto_step = 0.1; |
| mdavis30 | 6:ce2cf7f4d7d5 | 128 | float volume_bce = 1.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 129 | int bce_man_step = 50; |
| mdavis30 | 8:9b9431404db7 | 130 | |
| mdavis30 | 8:9b9431404db7 | 131 | float ticker_step_size = 1.0; |
| mdavis30 | 0:381a84fad08b | 132 | |
| mdavis30 | 0:381a84fad08b | 133 | //set the intial gains for the position controller |
| mdavis30 | 8:9b9431404db7 | 134 | posCon().setPgain(P); |
| mdavis30 | 0:381a84fad08b | 135 | posCon().setIgain(I); |
| mdavis30 | 0:381a84fad08b | 136 | posCon().setDgain(D); |
| mdavis30 | 6:ce2cf7f4d7d5 | 137 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 138 | hBridge().reset(); |
| mdavis30 | 8:9b9431404db7 | 139 | //hBridge().run(motor_cmd); |
| mdavis30 | 8:9b9431404db7 | 140 | |
| mdavis30 | 0:381a84fad08b | 141 | /* *************************** LED *************************** */ |
| mdavis30 | 0:381a84fad08b | 142 | led1 = 1; //initial values |
| mdavis30 | 0:381a84fad08b | 143 | led2 = 1; |
| mdavis30 | 6:ce2cf7f4d7d5 | 144 | led3 = 1; |
| mdavis30 | 0:381a84fad08b | 145 | led4 = 1; |
| mdavis30 | 0:381a84fad08b | 146 | /* *************************** LED *************************** */ |
| mdavis30 | 6:ce2cf7f4d7d5 | 147 | |
| mdavis30 | 8:9b9431404db7 | 148 | IMU_code IMU_object; |
| mdavis30 | 0:381a84fad08b | 149 | |
| mdavis30 | 0:381a84fad08b | 150 | while(1) |
| mdavis30 | 0:381a84fad08b | 151 | { |
| mdavis30 | 0:381a84fad08b | 152 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 153 | IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop |
| mdavis30 | 6:ce2cf7f4d7d5 | 154 | IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly? |
| mdavis30 | 6:ce2cf7f4d7d5 | 155 | //PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_yaw_angle, la_setPoint); |
| mdavis30 | 0:381a84fad08b | 156 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 157 | |
| mdavis30 | 0:381a84fad08b | 158 | /* Buoyancy Engine */ |
| mdavis30 | 0:381a84fad08b | 159 | // update the position velocity filter |
| mdavis30 | 0:381a84fad08b | 160 | pvf().update(); |
| mdavis30 | 0:381a84fad08b | 161 | |
| mdavis30 | 0:381a84fad08b | 162 | //update the controller with the current numbers in the position guesser |
| mdavis30 | 0:381a84fad08b | 163 | posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ; |
| mdavis30 | 0:381a84fad08b | 164 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 0:381a84fad08b | 165 | |
| mdavis30 | 0:381a84fad08b | 166 | /* Buoyancy Engine */ |
| mdavis30 | 0:381a84fad08b | 167 | |
| mdavis30 | 0:381a84fad08b | 168 | //FOR DEBUGGING |
| mdavis30 | 0:381a84fad08b | 169 | //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 | 170 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 171 | //PC.printf("WHILE LOOP\n"); //DEBUG |
| mdavis30 | 0:381a84fad08b | 172 | if (PC.readable()) |
| mdavis30 | 0:381a84fad08b | 173 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 174 | //led4 != led4; |
| mdavis30 | 6:ce2cf7f4d7d5 | 175 | //PC.printf("DEBUG: PC IS READABLE\n"); //DEBUG |
| mdavis30 | 0:381a84fad08b | 176 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 177 | Key=PC.getc(); |
| mdavis30 | 4:3c22d85a94a8 | 178 | //Universal MBED Controls |
| mdavis30 | 6:ce2cf7f4d7d5 | 179 | if(Key=='!') //RESET THE MBED |
| mdavis30 | 0:381a84fad08b | 180 | { |
| mdavis30 | 4:3c22d85a94a8 | 181 | PC.printf("MBED RESET KEY (!) PRESSED\n"); |
| mdavis30 | 4:3c22d85a94a8 | 182 | PC.printf("Linear Actuator Motor disabled!\n"); |
| mdavis30 | 4:3c22d85a94a8 | 183 | //disable the motor |
| mdavis30 | 4:3c22d85a94a8 | 184 | BLA_object.Keyboard_Q(); //DISABLE THE MOTOR |
| mdavis30 | 4:3c22d85a94a8 | 185 | wait(0.5); //500 milliseconds |
| mdavis30 | 4:3c22d85a94a8 | 186 | mbed_reset(); //reset the mbed! |
| mdavis30 | 0:381a84fad08b | 187 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 188 | else if(Key =='H') //homing sequence |
| mdavis30 | 0:381a84fad08b | 189 | { |
| mdavis30 | 4:3c22d85a94a8 | 190 | PC.printf("### homing the device ###"); |
| mdavis30 | 4:3c22d85a94a8 | 191 | BLA_object.Keyboard_H(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 192 | wait(10); //for debugging |
| mdavis30 | 4:3c22d85a94a8 | 193 | |
| tnhnrl | 5:7421776f6b08 | 194 | const char *char_actual_position = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 195 | //actual_position_string = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 196 | |
| tnhnrl | 5:7421776f6b08 | 197 | sscanf(char_actual_position, "%lf", &double_actual_position); |
| tnhnrl | 5:7421776f6b08 | 198 | // 7/10/17 |
| tnhnrl | 5:7421776f6b08 | 199 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 200 | PC.printf("LA actual position: %lf \n", double_actual_position); |
| mdavis30 | 6:ce2cf7f4d7d5 | 201 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 202 | //TROY NOTES on Linear Actuator commands: |
| mdavis30 | 6:ce2cf7f4d7d5 | 203 | //"pos" returns integer value "66813 for example" |
| mdavis30 | 6:ce2cf7f4d7d5 | 204 | //"hosp-100" is the setting of the motor, it retracts at 100 rpm |
| mdavis30 | 6:ce2cf7f4d7d5 | 205 | //"gohoseq" to home it, it will not give you feedback |
| mdavis30 | 6:ce2cf7f4d7d5 | 206 | //print the position after you do this to see where the LA is |
| mdavis30 | 6:ce2cf7f4d7d5 | 207 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 208 | else if(Key=='p' or Key == 'P') |
| mdavis30 | 6:ce2cf7f4d7d5 | 209 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 210 | PC.printf("BCE piston position: %f velocity? %f (BCE active? %d)\n", pvf().getPosition(), pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY |
| tnhnrl | 5:7421776f6b08 | 211 | |
| tnhnrl | 5:7421776f6b08 | 212 | // 7/10/17 |
| tnhnrl | 5:7421776f6b08 | 213 | //actual_position_string = BLA_object.get_pos(); |
| tnhnrl | 5:7421776f6b08 | 214 | //actual_position_string = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 215 | //const char *char_actual_position = string_actual_position.c_str(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 216 | |
| tnhnrl | 5:7421776f6b08 | 217 | const char *char_actual_position = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 218 | //actual_position_string = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 219 | |
| tnhnrl | 5:7421776f6b08 | 220 | sscanf(char_actual_position, "%lf", &double_actual_position); |
| tnhnrl | 5:7421776f6b08 | 221 | // 7/10/17 |
| tnhnrl | 5:7421776f6b08 | 222 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 223 | PC.printf("### L.A. position is\nLA_AP: %lf ###\n", double_actual_position); //flip this back and forth |
| mdavis30 | 6:ce2cf7f4d7d5 | 224 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 225 | if (double_actual_position > 180000) |
| mdavis30 | 6:ce2cf7f4d7d5 | 226 | PC.printf(" <<<< REACHED MAXIMUM L.A. POSITION! >>>>\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 227 | else if (double_actual_position < 0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 228 | PC.printf(" <<<< REACHED MINIMUM L.A. POSITION! >>>>\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 229 | |
| mdavis30 | 4:3c22d85a94a8 | 230 | wait(1); //for debugging |
| mdavis30 | 6:ce2cf7f4d7d5 | 231 | // "-999999" means it is not working |
| mdavis30 | 3:1257a7d2eb3a | 232 | } |
| mdavis30 | 8:9b9431404db7 | 233 | else if (Key == '3' or Key == '#') |
| mdavis30 | 8:9b9431404db7 | 234 | { |
| mdavis30 | 8:9b9431404db7 | 235 | PC.printf("Ticker Sensor Step Size\n"); |
| mdavis30 | 8:9b9431404db7 | 236 | if (ticker_step_size == 1.0) |
| mdavis30 | 8:9b9431404db7 | 237 | { |
| mdavis30 | 8:9b9431404db7 | 238 | ticker_step_size = 5.0; |
| mdavis30 | 8:9b9431404db7 | 239 | } |
| mdavis30 | 8:9b9431404db7 | 240 | else if (ticker_step_size == 5.0) |
| mdavis30 | 8:9b9431404db7 | 241 | { |
| mdavis30 | 8:9b9431404db7 | 242 | ticker_step_size = 10.0; |
| mdavis30 | 8:9b9431404db7 | 243 | } |
| mdavis30 | 8:9b9431404db7 | 244 | else if (ticker_step_size == 10.0) |
| mdavis30 | 8:9b9431404db7 | 245 | { |
| mdavis30 | 8:9b9431404db7 | 246 | ticker_step_size = 20.0; |
| mdavis30 | 8:9b9431404db7 | 247 | } |
| mdavis30 | 8:9b9431404db7 | 248 | else if (ticker_step_size == 20.0) |
| mdavis30 | 8:9b9431404db7 | 249 | { |
| mdavis30 | 8:9b9431404db7 | 250 | ticker_step_size = 1.0; |
| mdavis30 | 8:9b9431404db7 | 251 | } |
| mdavis30 | 8:9b9431404db7 | 252 | PC.printf("Ticker Step Size is %f\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 253 | } |
| mdavis30 | 8:9b9431404db7 | 254 | else if (Key =='4' or Key == '$') |
| mdavis30 | 8:9b9431404db7 | 255 | { |
| mdavis30 | 8:9b9431404db7 | 256 | if (IMU_count == 0) |
| mdavis30 | 8:9b9431404db7 | 257 | { |
| mdavis30 | 8:9b9431404db7 | 258 | PC.printf("IMU turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 259 | IMU_ticker.attach(&IMU_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 260 | IMU_count = 1; |
| mdavis30 | 8:9b9431404db7 | 261 | } |
| mdavis30 | 8:9b9431404db7 | 262 | else if (IMU_count == 1) |
| mdavis30 | 8:9b9431404db7 | 263 | { |
| mdavis30 | 8:9b9431404db7 | 264 | PC.printf("IMU turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 265 | IMU_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 266 | IMU_count = 0; |
| mdavis30 | 8:9b9431404db7 | 267 | } |
| mdavis30 | 8:9b9431404db7 | 268 | |
| mdavis30 | 8:9b9431404db7 | 269 | } |
| mdavis30 | 8:9b9431404db7 | 270 | else if (Key == '5' or Key == '%') |
| mdavis30 | 8:9b9431404db7 | 271 | { |
| mdavis30 | 8:9b9431404db7 | 272 | if (Pr_count == 0) |
| mdavis30 | 8:9b9431404db7 | 273 | { |
| mdavis30 | 8:9b9431404db7 | 274 | PC.printf("Pressure turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 275 | PRESSURE_ticker.attach(&PRESSURE_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 276 | Pr_count = 1; |
| mdavis30 | 8:9b9431404db7 | 277 | } |
| mdavis30 | 8:9b9431404db7 | 278 | else if (Pr_count == 1) |
| mdavis30 | 8:9b9431404db7 | 279 | { |
| mdavis30 | 8:9b9431404db7 | 280 | PC.printf("Pressure turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 281 | PRESSURE_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 282 | Pr_count = 0; |
| mdavis30 | 8:9b9431404db7 | 283 | } |
| mdavis30 | 8:9b9431404db7 | 284 | } |
| mdavis30 | 8:9b9431404db7 | 285 | else if (Key == '6' or Key == '^') |
| mdavis30 | 8:9b9431404db7 | 286 | { |
| mdavis30 | 8:9b9431404db7 | 287 | if (BCE_count == 0) |
| mdavis30 | 8:9b9431404db7 | 288 | { |
| mdavis30 | 8:9b9431404db7 | 289 | PC.printf("BCE turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 290 | BCE_ticker.attach(&BCE_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 291 | BCE_count = 1; |
| mdavis30 | 8:9b9431404db7 | 292 | } |
| mdavis30 | 8:9b9431404db7 | 293 | else if (BCE_count == 1) |
| mdavis30 | 8:9b9431404db7 | 294 | { |
| mdavis30 | 8:9b9431404db7 | 295 | PC.printf("BCE turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 296 | BCE_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 297 | BCE_count = 0; |
| mdavis30 | 8:9b9431404db7 | 298 | } |
| mdavis30 | 8:9b9431404db7 | 299 | } |
| mdavis30 | 4:3c22d85a94a8 | 300 | //Buoyancy Engine Controls |
| mdavis30 | 4:3c22d85a94a8 | 301 | else if (Key == ',' or Key == '<') |
| mdavis30 | 4:3c22d85a94a8 | 302 | { |
| mdavis30 | 4:3c22d85a94a8 | 303 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 304 | { |
| mdavis30 | 4:3c22d85a94a8 | 305 | PC.printf("BCE: Now in Automatic Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 306 | BCE_auto = true; |
| mdavis30 | 4:3c22d85a94a8 | 307 | } |
| mdavis30 | 4:3c22d85a94a8 | 308 | else |
| mdavis30 | 4:3c22d85a94a8 | 309 | { |
| tnhnrl | 5:7421776f6b08 | 310 | PC.printf("BCE: Still in Automatic Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 311 | } |
| mdavis30 | 4:3c22d85a94a8 | 312 | } |
| mdavis30 | 4:3c22d85a94a8 | 313 | else if (Key == '.' or Key == '>') |
| mdavis30 | 4:3c22d85a94a8 | 314 | { |
| mdavis30 | 4:3c22d85a94a8 | 315 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 316 | { |
| mdavis30 | 4:3c22d85a94a8 | 317 | PC.printf("BCE: Now in Manual Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 318 | BCE_auto = false; |
| mdavis30 | 4:3c22d85a94a8 | 319 | } |
| mdavis30 | 4:3c22d85a94a8 | 320 | else |
| mdavis30 | 4:3c22d85a94a8 | 321 | { |
| tnhnrl | 5:7421776f6b08 | 322 | PC.printf("BCE: Still in Manual Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 323 | } |
| mdavis30 | 4:3c22d85a94a8 | 324 | } |
| mdavis30 | 4:3c22d85a94a8 | 325 | //BCE Automatic Controls |
| mdavis30 | 6:ce2cf7f4d7d5 | 326 | else if(Key =='d' or Key == 'D') |
| mdavis30 | 4:3c22d85a94a8 | 327 | { |
| mdavis30 | 4:3c22d85a94a8 | 328 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 329 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 330 | volume_bce -= bce_auto_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 331 | PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 332 | } |
| mdavis30 | 4:3c22d85a94a8 | 333 | else |
| mdavis30 | 4:3c22d85a94a8 | 334 | { |
| mdavis30 | 4:3c22d85a94a8 | 335 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 336 | } |
| mdavis30 | 4:3c22d85a94a8 | 337 | } |
| mdavis30 | 4:3c22d85a94a8 | 338 | else if(Key == 'f' or Key == 'F') |
| mdavis30 | 4:3c22d85a94a8 | 339 | { |
| mdavis30 | 4:3c22d85a94a8 | 340 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 341 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 342 | volume_bce += bce_auto_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 343 | PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 344 | } |
| mdavis30 | 4:3c22d85a94a8 | 345 | else |
| mdavis30 | 4:3c22d85a94a8 | 346 | { |
| mdavis30 | 4:3c22d85a94a8 | 347 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 348 | } |
| mdavis30 | 4:3c22d85a94a8 | 349 | } |
| mdavis30 | 4:3c22d85a94a8 | 350 | else if(Key == 'r' or Key == 'R') |
| mdavis30 | 4:3c22d85a94a8 | 351 | { |
| mdavis30 | 4:3c22d85a94a8 | 352 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 353 | { |
| mdavis30 | 4:3c22d85a94a8 | 354 | PC.printf("\nR received!\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 355 | positionCmd_cm=(1000/(16*pi))*volume_bce; |
| mdavis30 | 6:ce2cf7f4d7d5 | 356 | positionCmd = positionCmd_cm*10; |
| mdavis30 | 6:ce2cf7f4d7d5 | 357 | //positionCmd= positionCmd_temp*0.000000001; |
| mdavis30 | 6:ce2cf7f4d7d5 | 358 | //PC.printf("BCE engine going to position: %3.2f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 359 | PC.printf("\nBASETP: %3.0f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 360 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 361 | //posCon().setPgain(P); |
| mdavis30 | 6:ce2cf7f4d7d5 | 362 | //posCon().setIgain(I); |
| mdavis30 | 6:ce2cf7f4d7d5 | 363 | //posCon().setDgain(D); |
| mdavis30 | 6:ce2cf7f4d7d5 | 364 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 6:ce2cf7f4d7d5 | 365 | |
| mdavis30 | 4:3c22d85a94a8 | 366 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 367 | count = 0; |
| mdavis30 | 4:3c22d85a94a8 | 368 | } |
| mdavis30 | 4:3c22d85a94a8 | 369 | else |
| mdavis30 | 4:3c22d85a94a8 | 370 | { |
| mdavis30 | 4:3c22d85a94a8 | 371 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 372 | } |
| mdavis30 | 4:3c22d85a94a8 | 373 | } |
| mdavis30 | 4:3c22d85a94a8 | 374 | //BCE Manual Controls |
| mdavis30 | 4:3c22d85a94a8 | 375 | else if (Key == '2' or Key == '@') |
| mdavis30 | 4:3c22d85a94a8 | 376 | { |
| mdavis30 | 4:3c22d85a94a8 | 377 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 378 | { |
| mdavis30 | 4:3c22d85a94a8 | 379 | PC.printf("BCE Manual Step Size Change\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 380 | if (bce_man_step == 50) |
| mdavis30 | 4:3c22d85a94a8 | 381 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 382 | bce_man_step = 25; |
| mdavis30 | 4:3c22d85a94a8 | 383 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 384 | else if (bce_man_step == 25) |
| mdavis30 | 4:3c22d85a94a8 | 385 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 386 | bce_man_step = 10; |
| mdavis30 | 4:3c22d85a94a8 | 387 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 388 | else if (bce_man_step == 10) |
| mdavis30 | 4:3c22d85a94a8 | 389 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 390 | bce_man_step = 1; |
| mdavis30 | 4:3c22d85a94a8 | 391 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 392 | else if (bce_man_step == 1) |
| mdavis30 | 4:3c22d85a94a8 | 393 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 394 | bce_man_step = 50; |
| mdavis30 | 4:3c22d85a94a8 | 395 | } |
| mdavis30 | 4:3c22d85a94a8 | 396 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 397 | PC.printf("BCE Manual Step Size Now %d\n", bce_man_step); |
| mdavis30 | 4:3c22d85a94a8 | 398 | } |
| mdavis30 | 4:3c22d85a94a8 | 399 | else |
| mdavis30 | 4:3c22d85a94a8 | 400 | { |
| mdavis30 | 4:3c22d85a94a8 | 401 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 402 | } |
| mdavis30 | 4:3c22d85a94a8 | 403 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 404 | else if (Key == 'z' or Key =='Z') |
| mdavis30 | 6:ce2cf7f4d7d5 | 405 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 406 | if (BCE_auto == false and positionCmd < 395) |
| mdavis30 | 6:ce2cf7f4d7d5 | 407 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 408 | //increment the duty cycle |
| mdavis30 | 6:ce2cf7f4d7d5 | 409 | positionCmd -= bce_man_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 410 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 6:ce2cf7f4d7d5 | 411 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 412 | else if (positionCmd > 395) |
| mdavis30 | 6:ce2cf7f4d7d5 | 413 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 414 | PC.printf("ERROR: Cannot move past 395 mm\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 415 | positionCmd = 370; |
| mdavis30 | 6:ce2cf7f4d7d5 | 416 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 417 | else |
| mdavis30 | 6:ce2cf7f4d7d5 | 418 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 419 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 420 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 421 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 422 | else if (Key == 'x' or Key == 'X') |
| mdavis30 | 6:ce2cf7f4d7d5 | 423 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 424 | if (BCE_auto == false and positionCmd > -25) |
| mdavis30 | 6:ce2cf7f4d7d5 | 425 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 426 | //decrement the duty cycle |
| mdavis30 | 6:ce2cf7f4d7d5 | 427 | positionCmd += bce_man_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 428 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 6:ce2cf7f4d7d5 | 429 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 430 | else if (positionCmd < -25) |
| mdavis30 | 6:ce2cf7f4d7d5 | 431 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 432 | PC.printf("ERROR: Cannot move past -5 mm\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 433 | positionCmd = -5; |
| mdavis30 | 6:ce2cf7f4d7d5 | 434 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 435 | else |
| mdavis30 | 6:ce2cf7f4d7d5 | 436 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 437 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 438 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 439 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 440 | else if(Key=='w' or Key =='W') |
| mdavis30 | 4:3c22d85a94a8 | 441 | { |
| mdavis30 | 4:3c22d85a94a8 | 442 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 443 | { |
| mdavis30 | 4:3c22d85a94a8 | 444 | PC.printf("\nW received!\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 445 | PC.printf("BASETP: %3.0f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 446 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 4:3c22d85a94a8 | 447 | //posCon().setPgain(P); |
| mdavis30 | 4:3c22d85a94a8 | 448 | //posCon().setIgain(I); |
| mdavis30 | 4:3c22d85a94a8 | 449 | //posCon().setDgain(D); |
| mdavis30 | 4:3c22d85a94a8 | 450 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 4:3c22d85a94a8 | 451 | |
| mdavis30 | 4:3c22d85a94a8 | 452 | hBridge().reset(); |
| mdavis30 | 4:3c22d85a94a8 | 453 | |
| mdavis30 | 4:3c22d85a94a8 | 454 | count = 0; |
| mdavis30 | 4:3c22d85a94a8 | 455 | |
| mdavis30 | 4:3c22d85a94a8 | 456 | } |
| mdavis30 | 4:3c22d85a94a8 | 457 | else |
| mdavis30 | 4:3c22d85a94a8 | 458 | { |
| mdavis30 | 4:3c22d85a94a8 | 459 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 460 | } |
| mdavis30 | 4:3c22d85a94a8 | 461 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 462 | else if (Key == 'l' or Key == 'L') |
| mdavis30 | 8:9b9431404db7 | 463 | 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 | 464 | |
| mdavis30 | 4:3c22d85a94a8 | 465 | //Linear Actuator Controls |
| mdavis30 | 4:3c22d85a94a8 | 466 | else if(Key == 'c' or Key == 'C') |
| mdavis30 | 4:3c22d85a94a8 | 467 | { |
| mdavis30 | 4:3c22d85a94a8 | 468 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 469 | { |
| mdavis30 | 4:3c22d85a94a8 | 470 | PC.printf("ERROR: LA already in auto mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 471 | } |
| mdavis30 | 4:3c22d85a94a8 | 472 | else |
| mdavis30 | 4:3c22d85a94a8 | 473 | { |
| mdavis30 | 4:3c22d85a94a8 | 474 | LA_auto = true; |
| mdavis30 | 6:ce2cf7f4d7d5 | 475 | PC.printf("```````````Now in IMU Controlled Mode```````````````\n"); |
| mdavis30 | 4:3c22d85a94a8 | 476 | count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 477 | } |
| mdavis30 | 4:3c22d85a94a8 | 478 | } |
| mdavis30 | 4:3c22d85a94a8 | 479 | else if (Key == 'v' or Key == 'V') |
| mdavis30 | 4:3c22d85a94a8 | 480 | { |
| mdavis30 | 4:3c22d85a94a8 | 481 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 482 | { |
| mdavis30 | 4:3c22d85a94a8 | 483 | LA_auto = false; |
| mdavis30 | 6:ce2cf7f4d7d5 | 484 | //go from imu controlled to manual |
| mdavis30 | 6:ce2cf7f4d7d5 | 485 | PC.printf("```````````Now in Manual Mode````````````````````\n"); |
| mdavis30 | 4:3c22d85a94a8 | 486 | count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 487 | } |
| mdavis30 | 4:3c22d85a94a8 | 488 | else |
| mdavis30 | 4:3c22d85a94a8 | 489 | { |
| mdavis30 | 4:3c22d85a94a8 | 490 | PC.printf("ERROR: LA already in manual mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 491 | } |
| mdavis30 | 4:3c22d85a94a8 | 492 | } |
| mdavis30 | 4:3c22d85a94a8 | 493 | else if (Key == '0' or Key == ')') |
| mdavis30 | 4:3c22d85a94a8 | 494 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 495 | PC.printf(") recieved\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 496 | if (la_step == 0.5) |
| mdavis30 | 6:ce2cf7f4d7d5 | 497 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 498 | la_step = 1.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 499 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 500 | else if (la_step == 1.0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 501 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 502 | la_step = 5.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 503 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 504 | else if (la_step == 5.0) |
| mdavis30 | 4:3c22d85a94a8 | 505 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 506 | la_step = 10.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 507 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 508 | else if (la_step == 10.0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 509 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 510 | la_step = 15.0; |
| mdavis30 | 4:3c22d85a94a8 | 511 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 512 | else if (la_step == 15.0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 513 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 514 | la_step = 0.5; |
| mdavis30 | 4:3c22d85a94a8 | 515 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 516 | PC.printf("LA Step Size Now %f\n", la_step); |
| tnhnrl | 5:7421776f6b08 | 517 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 518 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 519 | else if (Key=='-' or Key == '_') |
| tnhnrl | 5:7421776f6b08 | 520 | { |
| tnhnrl | 5:7421776f6b08 | 521 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 522 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 523 | la_setPoint_temp -= la_step; //IMU_yaw_angle -= 1.0; |
| tnhnrl | 5:7421776f6b08 | 524 | PC.printf("- recieved\n"); |
| tnhnrl | 5:7421776f6b08 | 525 | PC.printf("LA auto step size: %f\n", la_step); |
| tnhnrl | 5:7421776f6b08 | 526 | PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp); |
| mdavis30 | 4:3c22d85a94a8 | 527 | } |
| tnhnrl | 5:7421776f6b08 | 528 | else |
| mdavis30 | 4:3c22d85a94a8 | 529 | { |
| tnhnrl | 5:7421776f6b08 | 530 | PC.printf("ERROR: LA in manual mode!\n"); |
| mdavis30 | 4:3c22d85a94a8 | 531 | } |
| mdavis30 | 4:3c22d85a94a8 | 532 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 533 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 534 | else if (Key =='=' or Key == '+') |
| tnhnrl | 5:7421776f6b08 | 535 | { |
| tnhnrl | 5:7421776f6b08 | 536 | if (LA_auto == true) |
| tnhnrl | 5:7421776f6b08 | 537 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 538 | la_setPoint_temp += la_step; //IMU_yaw_angle += 1.0; |
| tnhnrl | 5:7421776f6b08 | 539 | PC.printf("+ recieved\n"); |
| tnhnrl | 5:7421776f6b08 | 540 | PC.printf("LA auto step size: %f\n", la_step); |
| tnhnrl | 5:7421776f6b08 | 541 | PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp); |
| tnhnrl | 5:7421776f6b08 | 542 | } |
| tnhnrl | 5:7421776f6b08 | 543 | else |
| tnhnrl | 5:7421776f6b08 | 544 | { |
| tnhnrl | 5:7421776f6b08 | 545 | PC.printf("ERROR: LA in manual mode!\n"); |
| tnhnrl | 5:7421776f6b08 | 546 | } |
| tnhnrl | 5:7421776f6b08 | 547 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 548 | |
| mdavis30 | 3:1257a7d2eb3a | 549 | else if (Key == 'A' or Key == 'a') |
| mdavis30 | 6:ce2cf7f4d7d5 | 550 | { |
| tnhnrl | 5:7421776f6b08 | 551 | if (LA_auto == true) |
| tnhnrl | 5:7421776f6b08 | 552 | { |
| tnhnrl | 5:7421776f6b08 | 553 | PC.printf("A recieved\n"); |
| tnhnrl | 5:7421776f6b08 | 554 | la_setPoint=la_setPoint_temp; |
| tnhnrl | 5:7421776f6b08 | 555 | PC.printf("LA angle now set to\nLA_A_SND: %f\n", la_setPoint); |
| tnhnrl | 5:7421776f6b08 | 556 | } |
| tnhnrl | 5:7421776f6b08 | 557 | else |
| tnhnrl | 5:7421776f6b08 | 558 | { |
| tnhnrl | 5:7421776f6b08 | 559 | PC.printf("ERROR: LA in manual mode!\n"); |
| tnhnrl | 5:7421776f6b08 | 560 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 561 | } |
| mdavis30 | 4:3c22d85a94a8 | 562 | |
| mdavis30 | 0:381a84fad08b | 563 | |
| mdavis30 | 4:3c22d85a94a8 | 564 | else if(Key == 'n' or Key == 'N') |
| mdavis30 | 6:ce2cf7f4d7d5 | 565 | { |
| tnhnrl | 5:7421776f6b08 | 566 | if (LA_auto == false) |
| tnhnrl | 5:7421776f6b08 | 567 | { |
| tnhnrl | 5:7421776f6b08 | 568 | PC.printf("N key pressed. \n"); |
| tnhnrl | 5:7421776f6b08 | 569 | PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY()); |
| tnhnrl | 5:7421776f6b08 | 570 | } |
| tnhnrl | 5:7421776f6b08 | 571 | |
| tnhnrl | 5:7421776f6b08 | 572 | else |
| tnhnrl | 5:7421776f6b08 | 573 | { |
| tnhnrl | 5:7421776f6b08 | 574 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 575 | } |
| mdavis30 | 4:3c22d85a94a8 | 576 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 577 | |
| mdavis30 | 4:3c22d85a94a8 | 578 | else if(Key == 'm' or Key == 'M') |
| mdavis30 | 4:3c22d85a94a8 | 579 | { |
| tnhnrl | 5:7421776f6b08 | 580 | if (LA_auto == false) |
| tnhnrl | 5:7421776f6b08 | 581 | { |
| tnhnrl | 5:7421776f6b08 | 582 | PC.printf("M key pressed. \n"); |
| tnhnrl | 5:7421776f6b08 | 583 | PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY()); |
| tnhnrl | 5:7421776f6b08 | 584 | } |
| tnhnrl | 5:7421776f6b08 | 585 | |
| tnhnrl | 5:7421776f6b08 | 586 | else |
| tnhnrl | 5:7421776f6b08 | 587 | { |
| tnhnrl | 5:7421776f6b08 | 588 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 589 | } |
| mdavis30 | 0:381a84fad08b | 590 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 591 | |
| mdavis30 | 4:3c22d85a94a8 | 592 | else if(Key == 'j' or Key == 'J') |
| mdavis30 | 4:3c22d85a94a8 | 593 | { |
| tnhnrl | 5:7421776f6b08 | 594 | if (LA_auto == false) |
| tnhnrl | 5:7421776f6b08 | 595 | { |
| tnhnrl | 5:7421776f6b08 | 596 | PC.printf("J key pressed. \n"); |
| tnhnrl | 5:7421776f6b08 | 597 | PC.printf("%s\n", BLA_object.Keyboard_A()); |
| tnhnrl | 5:7421776f6b08 | 598 | } |
| tnhnrl | 5:7421776f6b08 | 599 | |
| tnhnrl | 5:7421776f6b08 | 600 | else |
| tnhnrl | 5:7421776f6b08 | 601 | { |
| tnhnrl | 5:7421776f6b08 | 602 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 603 | } |
| mdavis30 | 0:381a84fad08b | 604 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 605 | |
| mdavis30 | 4:3c22d85a94a8 | 606 | else if(Key == 'k' or Key == 'K') |
| mdavis30 | 4:3c22d85a94a8 | 607 | { |
| tnhnrl | 5:7421776f6b08 | 608 | if (LA_auto == false) |
| tnhnrl | 5:7421776f6b08 | 609 | { |
| tnhnrl | 5:7421776f6b08 | 610 | PC.printf("K key pressed. \n"); |
| tnhnrl | 5:7421776f6b08 | 611 | PC.printf("%s\n", BLA_object.Keyboard_D()); |
| tnhnrl | 5:7421776f6b08 | 612 | } |
| tnhnrl | 5:7421776f6b08 | 613 | |
| tnhnrl | 5:7421776f6b08 | 614 | else |
| tnhnrl | 5:7421776f6b08 | 615 | { |
| tnhnrl | 5:7421776f6b08 | 616 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 617 | } |
| tnhnrl | 5:7421776f6b08 | 618 | } |
| mdavis30 | 0:381a84fad08b | 619 | |
| mdavis30 | 0:381a84fad08b | 620 | else |
| mdavis30 | 0:381a84fad08b | 621 | { |
| mdavis30 | 0:381a84fad08b | 622 | PC.printf("\n%c received!\n", Key); |
| mdavis30 | 0:381a84fad08b | 623 | PC.printf("\nDoing nothing.\n"); |
| mdavis30 | 0:381a84fad08b | 624 | } |
| mdavis30 | 0:381a84fad08b | 625 | |
| mdavis30 | 0:381a84fad08b | 626 | wait_us(100); //for PC readable |
| mdavis30 | 6:ce2cf7f4d7d5 | 627 | //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 | 628 | //BLA_object.PID_velocity_control(la_setPoint, IMU_yaw_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| tnhnrl | 5:7421776f6b08 | 629 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 630 | //end of PC readBLE |
| mdavis30 | 6:ce2cf7f4d7d5 | 631 | |
| tnhnrl | 5:7421776f6b08 | 632 | //MC readable |
| tnhnrl | 5:7421776f6b08 | 633 | MC_readable_string = BLA_object.MC_readable_redux(); |
| tnhnrl | 5:7421776f6b08 | 634 | //PC.printf("CHECK_MC_readable:\n%s\n", MC_readable_string); |
| mdavis30 | 0:381a84fad08b | 635 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 636 | /* if (LA_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 637 | { |
| tnhnrl | 5:7421776f6b08 | 638 | if (!MC_readable_string.empty()) //if this string is empty |
| tnhnrl | 5:7421776f6b08 | 639 | { |
| tnhnrl | 5:7421776f6b08 | 640 | PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller |
| tnhnrl | 5:7421776f6b08 | 641 | } |
| tnhnrl | 5:7421776f6b08 | 642 | else |
| tnhnrl | 5:7421776f6b08 | 643 | { |
| tnhnrl | 5:7421776f6b08 | 644 | ; |
| tnhnrl | 5:7421776f6b08 | 645 | //PC.printf("NOTHING?\n"); |
| tnhnrl | 5:7421776f6b08 | 646 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 647 | }*/ |
| mdavis30 | 6:ce2cf7f4d7d5 | 648 | if (LA_auto==true) |
| tnhnrl | 5:7421776f6b08 | 649 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 650 | //PC.printf("LA_auto true\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 651 | //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 | 6:ce2cf7f4d7d5 | 652 | BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 653 | //wait_us(100); //for PC readable (0.1 ms) |
| tnhnrl | 5:7421776f6b08 | 654 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 655 | else if (LA_auto == false) |
| mdavis30 | 6:ce2cf7f4d7d5 | 656 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 657 | //PC.printf("LA_auto false\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 658 | // if (!MC_readable_string.empty()) //if this string is empty |
| mdavis30 | 6:ce2cf7f4d7d5 | 659 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 660 | // PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller |
| mdavis30 | 6:ce2cf7f4d7d5 | 661 | // } |
| mdavis30 | 6:ce2cf7f4d7d5 | 662 | // else |
| mdavis30 | 6:ce2cf7f4d7d5 | 663 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 664 | // ; |
| mdavis30 | 6:ce2cf7f4d7d5 | 665 | // //PC.printf("NOTHING?\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 666 | // } |
| mdavis30 | 4:3c22d85a94a8 | 667 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 668 | // while (count_while==0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 669 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 670 | //// PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled |
| mdavis30 | 6:ce2cf7f4d7d5 | 671 | //// PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 672 | //// wait(1); |
| mdavis30 | 6:ce2cf7f4d7d5 | 673 | //// PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 674 | //// wait(1); |
| mdavis30 | 6:ce2cf7f4d7d5 | 675 | //// PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 676 | //// count_while++; |
| mdavis30 | 6:ce2cf7f4d7d5 | 677 | // |
| mdavis30 | 6:ce2cf7f4d7d5 | 678 | // BLA_object.Keyboard_E(); //turn on motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 679 | // BLA_object.velocity_only(0); //set the velocity to zero just in case |
| mdavis30 | 6:ce2cf7f4d7d5 | 680 | // PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 681 | // count_while++; |
| mdavis30 | 6:ce2cf7f4d7d5 | 682 | // } |
| mdavis30 | 4:3c22d85a94a8 | 683 | } |
| mdavis30 | 4:3c22d85a94a8 | 684 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 685 | |
| mdavis30 | 0:381a84fad08b | 686 | if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0)) |
| mdavis30 | 0:381a84fad08b | 687 | { |
| mdavis30 | 0:381a84fad08b | 688 | count ++; |
| mdavis30 | 0:381a84fad08b | 689 | //pc().printf("We have a small issue\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 690 | if(count==10) |
| mdavis30 | 6:ce2cf7f4d7d5 | 691 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 692 | pc().printf("Bad pot issue\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 693 | //hBridge().stop(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 694 | count = 0; |
| mdavis30 | 7:10d7fbac30ea | 695 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 696 | } |
| mdavis30 | 0:381a84fad08b | 697 | |
| mdavis30 | 0:381a84fad08b | 698 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 699 | else if ((5.0*ain.read())<1.0) |
| mdavis30 | 2:c3cb3ea3c9fa | 700 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 701 | pc().printf("Hit the limit switch??\n"); |
| mdavis30 | 2:c3cb3ea3c9fa | 702 | hBridge().stop(); |
| mdavis30 | 7:10d7fbac30ea | 703 | wait(1); |
| mdavis30 | 7:10d7fbac30ea | 704 | hBridge().reset(); |
| mdavis30 | 7:10d7fbac30ea | 705 | positionCmd= 375; |
| mdavis30 | 7:10d7fbac30ea | 706 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 7:10d7fbac30ea | 707 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 8:9b9431404db7 | 708 | wait(2); |
| mdavis30 | 2:c3cb3ea3c9fa | 709 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 710 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 711 | //string snaps |
| mdavis30 | 0:381a84fad08b | 712 | else if (pvf().getVelocity() > 100) |
| mdavis30 | 0:381a84fad08b | 713 | { |
| tnhnrl | 5:7421776f6b08 | 714 | PC.printf("DEBUG: String position? %f velocity? %f\n", pvf().getPosition(), pvf().getVelocity()); //DEBUG TROY |
| mdavis30 | 0:381a84fad08b | 715 | //hBridge().stop(); |
| tnhnrl | 5:7421776f6b08 | 716 | //PC.printf("PosVelFilter B.E. Velocity: %f\n", pvf().getVelocity()); |
| mdavis30 | 6:ce2cf7f4d7d5 | 717 | PC.printf("********** String broke? *********\n"); |
| mdavis30 | 4:3c22d85a94a8 | 718 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 719 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 720 | } |
| mdavis30 | 0:381a84fad08b | 721 | } |