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@9:2dcfd6ce61ea, 2017-07-26 (annotated)
- Committer:
- mdavis30
- Date:
- Wed Jul 26 14:11:08 2017 +0000
- Revision:
- 9:2dcfd6ce61ea
- Parent:
- 8:9b9431404db7
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 | 9:2dcfd6ce61ea | 188 | /* else if(Key == '[') |
| mdavis30 | 9:2dcfd6ce61ea | 189 | { |
| mdavis30 | 9:2dcfd6ce61ea | 190 | BLA_object.velocity_only(-100); //send speed -100 command to linear actuator (retract) |
| mdavis30 | 9:2dcfd6ce61ea | 191 | } |
| mdavis30 | 9:2dcfd6ce61ea | 192 | else if(Key == ']') |
| mdavis30 | 9:2dcfd6ce61ea | 193 | { |
| mdavis30 | 9:2dcfd6ce61ea | 194 | BLA_object.velocity_only(100); //send speed +100 command to linear actuator (extend) |
| mdavis30 | 9:2dcfd6ce61ea | 195 | } |
| mdavis30 | 9:2dcfd6ce61ea | 196 | else if(Key == 'u') |
| mdavis30 | 9:2dcfd6ce61ea | 197 | { |
| mdavis30 | 9:2dcfd6ce61ea | 198 | BLA_object.Keyboard_U(); //stop linear actuator |
| mdavis30 | 9:2dcfd6ce61ea | 199 | } */ |
| mdavis30 | 6:ce2cf7f4d7d5 | 200 | else if(Key =='H') //homing sequence |
| mdavis30 | 0:381a84fad08b | 201 | { |
| mdavis30 | 4:3c22d85a94a8 | 202 | PC.printf("### homing the device ###"); |
| mdavis30 | 4:3c22d85a94a8 | 203 | BLA_object.Keyboard_H(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 204 | wait(10); //for debugging |
| mdavis30 | 4:3c22d85a94a8 | 205 | |
| tnhnrl | 5:7421776f6b08 | 206 | const char *char_actual_position = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 207 | //actual_position_string = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 208 | |
| tnhnrl | 5:7421776f6b08 | 209 | sscanf(char_actual_position, "%lf", &double_actual_position); |
| tnhnrl | 5:7421776f6b08 | 210 | // 7/10/17 |
| tnhnrl | 5:7421776f6b08 | 211 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 212 | PC.printf("LA actual position: %lf \n", double_actual_position); |
| mdavis30 | 6:ce2cf7f4d7d5 | 213 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 214 | //TROY NOTES on Linear Actuator commands: |
| mdavis30 | 6:ce2cf7f4d7d5 | 215 | //"pos" returns integer value "66813 for example" |
| mdavis30 | 6:ce2cf7f4d7d5 | 216 | //"hosp-100" is the setting of the motor, it retracts at 100 rpm |
| mdavis30 | 6:ce2cf7f4d7d5 | 217 | //"gohoseq" to home it, it will not give you feedback |
| mdavis30 | 6:ce2cf7f4d7d5 | 218 | //print the position after you do this to see where the LA is |
| mdavis30 | 6:ce2cf7f4d7d5 | 219 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 220 | else if(Key=='p' or Key == 'P') |
| mdavis30 | 6:ce2cf7f4d7d5 | 221 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 222 | PC.printf("BCE piston position: %f velocity? %f (BCE active? %d)\n", pvf().getPosition(), pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY |
| tnhnrl | 5:7421776f6b08 | 223 | |
| tnhnrl | 5:7421776f6b08 | 224 | // 7/10/17 |
| tnhnrl | 5:7421776f6b08 | 225 | //actual_position_string = BLA_object.get_pos(); |
| tnhnrl | 5:7421776f6b08 | 226 | //actual_position_string = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 227 | //const char *char_actual_position = string_actual_position.c_str(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 228 | |
| tnhnrl | 5:7421776f6b08 | 229 | const char *char_actual_position = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 230 | //actual_position_string = BLA_object.get_pos().c_str(); |
| tnhnrl | 5:7421776f6b08 | 231 | |
| tnhnrl | 5:7421776f6b08 | 232 | sscanf(char_actual_position, "%lf", &double_actual_position); |
| tnhnrl | 5:7421776f6b08 | 233 | // 7/10/17 |
| tnhnrl | 5:7421776f6b08 | 234 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 235 | PC.printf("### L.A. position is\nLA_AP: %lf ###\n", double_actual_position); //flip this back and forth |
| mdavis30 | 6:ce2cf7f4d7d5 | 236 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 237 | if (double_actual_position > 180000) |
| mdavis30 | 6:ce2cf7f4d7d5 | 238 | PC.printf(" <<<< REACHED MAXIMUM L.A. POSITION! >>>>\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 239 | else if (double_actual_position < 0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 240 | PC.printf(" <<<< REACHED MINIMUM L.A. POSITION! >>>>\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 241 | |
| mdavis30 | 4:3c22d85a94a8 | 242 | wait(1); //for debugging |
| mdavis30 | 6:ce2cf7f4d7d5 | 243 | // "-999999" means it is not working |
| mdavis30 | 3:1257a7d2eb3a | 244 | } |
| mdavis30 | 8:9b9431404db7 | 245 | else if (Key == '3' or Key == '#') |
| mdavis30 | 8:9b9431404db7 | 246 | { |
| mdavis30 | 8:9b9431404db7 | 247 | PC.printf("Ticker Sensor Step Size\n"); |
| mdavis30 | 8:9b9431404db7 | 248 | if (ticker_step_size == 1.0) |
| mdavis30 | 8:9b9431404db7 | 249 | { |
| mdavis30 | 8:9b9431404db7 | 250 | ticker_step_size = 5.0; |
| mdavis30 | 8:9b9431404db7 | 251 | } |
| mdavis30 | 8:9b9431404db7 | 252 | else if (ticker_step_size == 5.0) |
| mdavis30 | 8:9b9431404db7 | 253 | { |
| mdavis30 | 8:9b9431404db7 | 254 | ticker_step_size = 10.0; |
| mdavis30 | 8:9b9431404db7 | 255 | } |
| mdavis30 | 8:9b9431404db7 | 256 | else if (ticker_step_size == 10.0) |
| mdavis30 | 8:9b9431404db7 | 257 | { |
| mdavis30 | 8:9b9431404db7 | 258 | ticker_step_size = 20.0; |
| mdavis30 | 8:9b9431404db7 | 259 | } |
| mdavis30 | 8:9b9431404db7 | 260 | else if (ticker_step_size == 20.0) |
| mdavis30 | 8:9b9431404db7 | 261 | { |
| mdavis30 | 8:9b9431404db7 | 262 | ticker_step_size = 1.0; |
| mdavis30 | 8:9b9431404db7 | 263 | } |
| mdavis30 | 8:9b9431404db7 | 264 | PC.printf("Ticker Step Size is %f\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 265 | } |
| mdavis30 | 8:9b9431404db7 | 266 | else if (Key =='4' or Key == '$') |
| mdavis30 | 8:9b9431404db7 | 267 | { |
| mdavis30 | 8:9b9431404db7 | 268 | if (IMU_count == 0) |
| mdavis30 | 8:9b9431404db7 | 269 | { |
| mdavis30 | 8:9b9431404db7 | 270 | PC.printf("IMU turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 271 | IMU_ticker.attach(&IMU_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 272 | IMU_count = 1; |
| mdavis30 | 8:9b9431404db7 | 273 | } |
| mdavis30 | 8:9b9431404db7 | 274 | else if (IMU_count == 1) |
| mdavis30 | 8:9b9431404db7 | 275 | { |
| mdavis30 | 8:9b9431404db7 | 276 | PC.printf("IMU turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 277 | IMU_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 278 | IMU_count = 0; |
| mdavis30 | 8:9b9431404db7 | 279 | } |
| mdavis30 | 8:9b9431404db7 | 280 | |
| mdavis30 | 8:9b9431404db7 | 281 | } |
| mdavis30 | 8:9b9431404db7 | 282 | else if (Key == '5' or Key == '%') |
| mdavis30 | 8:9b9431404db7 | 283 | { |
| mdavis30 | 8:9b9431404db7 | 284 | if (Pr_count == 0) |
| mdavis30 | 8:9b9431404db7 | 285 | { |
| mdavis30 | 8:9b9431404db7 | 286 | PC.printf("Pressure turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 287 | PRESSURE_ticker.attach(&PRESSURE_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 288 | Pr_count = 1; |
| mdavis30 | 8:9b9431404db7 | 289 | } |
| mdavis30 | 8:9b9431404db7 | 290 | else if (Pr_count == 1) |
| mdavis30 | 8:9b9431404db7 | 291 | { |
| mdavis30 | 8:9b9431404db7 | 292 | PC.printf("Pressure turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 293 | PRESSURE_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 294 | Pr_count = 0; |
| mdavis30 | 8:9b9431404db7 | 295 | } |
| mdavis30 | 8:9b9431404db7 | 296 | } |
| mdavis30 | 8:9b9431404db7 | 297 | else if (Key == '6' or Key == '^') |
| mdavis30 | 8:9b9431404db7 | 298 | { |
| mdavis30 | 8:9b9431404db7 | 299 | if (BCE_count == 0) |
| mdavis30 | 8:9b9431404db7 | 300 | { |
| mdavis30 | 8:9b9431404db7 | 301 | PC.printf("BCE turned on! Ticker time is %f seconds\n", ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 302 | BCE_ticker.attach(&BCE_ticking, ticker_step_size); |
| mdavis30 | 8:9b9431404db7 | 303 | BCE_count = 1; |
| mdavis30 | 8:9b9431404db7 | 304 | } |
| mdavis30 | 8:9b9431404db7 | 305 | else if (BCE_count == 1) |
| mdavis30 | 8:9b9431404db7 | 306 | { |
| mdavis30 | 8:9b9431404db7 | 307 | PC.printf("BCE turned off!\n"); |
| mdavis30 | 8:9b9431404db7 | 308 | BCE_ticker.detach(); |
| mdavis30 | 8:9b9431404db7 | 309 | BCE_count = 0; |
| mdavis30 | 8:9b9431404db7 | 310 | } |
| mdavis30 | 8:9b9431404db7 | 311 | } |
| mdavis30 | 4:3c22d85a94a8 | 312 | //Buoyancy Engine Controls |
| mdavis30 | 4:3c22d85a94a8 | 313 | else if (Key == ',' 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: Now in Automatic Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 318 | BCE_auto = true; |
| mdavis30 | 4:3c22d85a94a8 | 319 | } |
| mdavis30 | 4:3c22d85a94a8 | 320 | else |
| mdavis30 | 4:3c22d85a94a8 | 321 | { |
| tnhnrl | 5:7421776f6b08 | 322 | PC.printf("BCE: Still in Automatic Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 323 | } |
| mdavis30 | 4:3c22d85a94a8 | 324 | } |
| mdavis30 | 4:3c22d85a94a8 | 325 | else if (Key == '.' or Key == '>') |
| mdavis30 | 4:3c22d85a94a8 | 326 | { |
| mdavis30 | 4:3c22d85a94a8 | 327 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 328 | { |
| mdavis30 | 4:3c22d85a94a8 | 329 | PC.printf("BCE: Now in Manual Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 330 | BCE_auto = false; |
| mdavis30 | 4:3c22d85a94a8 | 331 | } |
| mdavis30 | 4:3c22d85a94a8 | 332 | else |
| mdavis30 | 4:3c22d85a94a8 | 333 | { |
| tnhnrl | 5:7421776f6b08 | 334 | PC.printf("BCE: Still in Manual Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 335 | } |
| mdavis30 | 4:3c22d85a94a8 | 336 | } |
| mdavis30 | 4:3c22d85a94a8 | 337 | //BCE Automatic Controls |
| mdavis30 | 6:ce2cf7f4d7d5 | 338 | else if(Key =='d' or Key == 'D') |
| 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 == 'f' or Key == 'F') |
| mdavis30 | 4:3c22d85a94a8 | 351 | { |
| mdavis30 | 4:3c22d85a94a8 | 352 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 353 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 354 | volume_bce += bce_auto_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 355 | PC.printf("The volume for the buoyancy motor is\nVBE: %1.3f liters\n", volume_bce); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 356 | } |
| mdavis30 | 4:3c22d85a94a8 | 357 | else |
| mdavis30 | 4:3c22d85a94a8 | 358 | { |
| mdavis30 | 4:3c22d85a94a8 | 359 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 360 | } |
| mdavis30 | 4:3c22d85a94a8 | 361 | } |
| mdavis30 | 4:3c22d85a94a8 | 362 | else if(Key == 'r' or Key == 'R') |
| mdavis30 | 4:3c22d85a94a8 | 363 | { |
| mdavis30 | 4:3c22d85a94a8 | 364 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 365 | { |
| mdavis30 | 4:3c22d85a94a8 | 366 | PC.printf("\nR received!\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 367 | positionCmd_cm=(1000/(16*pi))*volume_bce; |
| mdavis30 | 6:ce2cf7f4d7d5 | 368 | positionCmd = positionCmd_cm*10; |
| mdavis30 | 6:ce2cf7f4d7d5 | 369 | //positionCmd= positionCmd_temp*0.000000001; |
| mdavis30 | 6:ce2cf7f4d7d5 | 370 | //PC.printf("BCE engine going to position: %3.2f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 371 | PC.printf("\nBASETP: %3.0f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 372 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 373 | //posCon().setPgain(P); |
| mdavis30 | 6:ce2cf7f4d7d5 | 374 | //posCon().setIgain(I); |
| mdavis30 | 6:ce2cf7f4d7d5 | 375 | //posCon().setDgain(D); |
| mdavis30 | 6:ce2cf7f4d7d5 | 376 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 6:ce2cf7f4d7d5 | 377 | |
| mdavis30 | 4:3c22d85a94a8 | 378 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 379 | count = 0; |
| mdavis30 | 4:3c22d85a94a8 | 380 | } |
| mdavis30 | 4:3c22d85a94a8 | 381 | else |
| mdavis30 | 4:3c22d85a94a8 | 382 | { |
| mdavis30 | 4:3c22d85a94a8 | 383 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 384 | } |
| mdavis30 | 4:3c22d85a94a8 | 385 | } |
| mdavis30 | 4:3c22d85a94a8 | 386 | //BCE Manual Controls |
| mdavis30 | 4:3c22d85a94a8 | 387 | else if (Key == '2' or Key == '@') |
| mdavis30 | 4:3c22d85a94a8 | 388 | { |
| mdavis30 | 4:3c22d85a94a8 | 389 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 390 | { |
| mdavis30 | 4:3c22d85a94a8 | 391 | PC.printf("BCE Manual Step Size Change\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 392 | if (bce_man_step == 50) |
| mdavis30 | 4:3c22d85a94a8 | 393 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 394 | bce_man_step = 25; |
| mdavis30 | 4:3c22d85a94a8 | 395 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 396 | else if (bce_man_step == 25) |
| mdavis30 | 4:3c22d85a94a8 | 397 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 398 | bce_man_step = 10; |
| mdavis30 | 4:3c22d85a94a8 | 399 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 400 | else if (bce_man_step == 10) |
| mdavis30 | 4:3c22d85a94a8 | 401 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 402 | bce_man_step = 1; |
| mdavis30 | 4:3c22d85a94a8 | 403 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 404 | else if (bce_man_step == 1) |
| mdavis30 | 4:3c22d85a94a8 | 405 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 406 | bce_man_step = 50; |
| mdavis30 | 4:3c22d85a94a8 | 407 | } |
| mdavis30 | 4:3c22d85a94a8 | 408 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 409 | PC.printf("BCE Manual Step Size Now %d\n", bce_man_step); |
| mdavis30 | 4:3c22d85a94a8 | 410 | } |
| mdavis30 | 4:3c22d85a94a8 | 411 | else |
| mdavis30 | 4:3c22d85a94a8 | 412 | { |
| mdavis30 | 4:3c22d85a94a8 | 413 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 414 | } |
| mdavis30 | 4:3c22d85a94a8 | 415 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 416 | else if (Key == 'z' or Key =='Z') |
| mdavis30 | 6:ce2cf7f4d7d5 | 417 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 418 | if (BCE_auto == false and positionCmd < 395) |
| mdavis30 | 6:ce2cf7f4d7d5 | 419 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 420 | //increment the duty cycle |
| mdavis30 | 6:ce2cf7f4d7d5 | 421 | positionCmd -= bce_man_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 422 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 6:ce2cf7f4d7d5 | 423 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 424 | else if (positionCmd > 395) |
| mdavis30 | 6:ce2cf7f4d7d5 | 425 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 426 | PC.printf("ERROR: Cannot move past 395 mm\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 427 | positionCmd = 370; |
| mdavis30 | 6:ce2cf7f4d7d5 | 428 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 429 | else |
| mdavis30 | 6:ce2cf7f4d7d5 | 430 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 431 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 432 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 433 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 434 | else if (Key == 'x' or Key == 'X') |
| mdavis30 | 6:ce2cf7f4d7d5 | 435 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 436 | if (BCE_auto == false and positionCmd > -25) |
| mdavis30 | 6:ce2cf7f4d7d5 | 437 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 438 | //decrement the duty cycle |
| mdavis30 | 6:ce2cf7f4d7d5 | 439 | positionCmd += bce_man_step; |
| mdavis30 | 6:ce2cf7f4d7d5 | 440 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 6:ce2cf7f4d7d5 | 441 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 442 | else if (positionCmd < -25) |
| mdavis30 | 6:ce2cf7f4d7d5 | 443 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 444 | PC.printf("ERROR: Cannot move past -5 mm\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 445 | positionCmd = -5; |
| mdavis30 | 6:ce2cf7f4d7d5 | 446 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 447 | else |
| mdavis30 | 6:ce2cf7f4d7d5 | 448 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 449 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 450 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 451 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 452 | else if(Key=='w' or Key =='W') |
| mdavis30 | 4:3c22d85a94a8 | 453 | { |
| mdavis30 | 4:3c22d85a94a8 | 454 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 455 | { |
| mdavis30 | 4:3c22d85a94a8 | 456 | PC.printf("\nW received!\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 457 | PC.printf("BASETP: %3.0f\n", positionCmd); |
| mdavis30 | 6:ce2cf7f4d7d5 | 458 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 4:3c22d85a94a8 | 459 | //posCon().setPgain(P); |
| mdavis30 | 4:3c22d85a94a8 | 460 | //posCon().setIgain(I); |
| mdavis30 | 4:3c22d85a94a8 | 461 | //posCon().setDgain(D); |
| mdavis30 | 4:3c22d85a94a8 | 462 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 4:3c22d85a94a8 | 463 | |
| mdavis30 | 4:3c22d85a94a8 | 464 | hBridge().reset(); |
| mdavis30 | 4:3c22d85a94a8 | 465 | |
| mdavis30 | 4:3c22d85a94a8 | 466 | count = 0; |
| mdavis30 | 4:3c22d85a94a8 | 467 | |
| mdavis30 | 4:3c22d85a94a8 | 468 | } |
| mdavis30 | 4:3c22d85a94a8 | 469 | else |
| mdavis30 | 4:3c22d85a94a8 | 470 | { |
| mdavis30 | 4:3c22d85a94a8 | 471 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 472 | } |
| mdavis30 | 4:3c22d85a94a8 | 473 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 474 | else if (Key == 'l' or Key == 'L') |
| mdavis30 | 8:9b9431404db7 | 475 | 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 | 476 | |
| mdavis30 | 4:3c22d85a94a8 | 477 | //Linear Actuator Controls |
| mdavis30 | 4:3c22d85a94a8 | 478 | else if(Key == 'c' or Key == 'C') |
| mdavis30 | 4:3c22d85a94a8 | 479 | { |
| mdavis30 | 4:3c22d85a94a8 | 480 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 481 | { |
| mdavis30 | 4:3c22d85a94a8 | 482 | PC.printf("ERROR: LA already in auto mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 483 | } |
| mdavis30 | 4:3c22d85a94a8 | 484 | else |
| mdavis30 | 4:3c22d85a94a8 | 485 | { |
| mdavis30 | 4:3c22d85a94a8 | 486 | LA_auto = true; |
| mdavis30 | 6:ce2cf7f4d7d5 | 487 | PC.printf("```````````Now in IMU Controlled Mode```````````````\n"); |
| mdavis30 | 4:3c22d85a94a8 | 488 | count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 489 | } |
| mdavis30 | 4:3c22d85a94a8 | 490 | } |
| mdavis30 | 4:3c22d85a94a8 | 491 | else if (Key == 'v' or Key == 'V') |
| mdavis30 | 4:3c22d85a94a8 | 492 | { |
| mdavis30 | 4:3c22d85a94a8 | 493 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 494 | { |
| mdavis30 | 4:3c22d85a94a8 | 495 | LA_auto = false; |
| mdavis30 | 6:ce2cf7f4d7d5 | 496 | //go from imu controlled to manual |
| mdavis30 | 6:ce2cf7f4d7d5 | 497 | PC.printf("```````````Now in Manual Mode````````````````````\n"); |
| mdavis30 | 4:3c22d85a94a8 | 498 | count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 499 | } |
| mdavis30 | 4:3c22d85a94a8 | 500 | else |
| mdavis30 | 4:3c22d85a94a8 | 501 | { |
| mdavis30 | 4:3c22d85a94a8 | 502 | PC.printf("ERROR: LA already in manual mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 503 | } |
| mdavis30 | 4:3c22d85a94a8 | 504 | } |
| mdavis30 | 4:3c22d85a94a8 | 505 | else if (Key == '0' or Key == ')') |
| mdavis30 | 4:3c22d85a94a8 | 506 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 507 | PC.printf(") recieved\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 508 | if (la_step == 0.5) |
| mdavis30 | 6:ce2cf7f4d7d5 | 509 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 510 | la_step = 1.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 511 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 512 | else if (la_step == 1.0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 513 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 514 | la_step = 5.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 515 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 516 | else if (la_step == 5.0) |
| mdavis30 | 4:3c22d85a94a8 | 517 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 518 | la_step = 10.0; |
| mdavis30 | 6:ce2cf7f4d7d5 | 519 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 520 | else if (la_step == 10.0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 521 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 522 | la_step = 15.0; |
| mdavis30 | 4:3c22d85a94a8 | 523 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 524 | else if (la_step == 15.0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 525 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 526 | la_step = 0.5; |
| mdavis30 | 4:3c22d85a94a8 | 527 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 528 | PC.printf("LA Step Size Now %f\n", la_step); |
| tnhnrl | 5:7421776f6b08 | 529 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 530 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 531 | else if (Key=='-' or Key == '_') |
| tnhnrl | 5:7421776f6b08 | 532 | { |
| tnhnrl | 5:7421776f6b08 | 533 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 534 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 535 | la_setPoint_temp -= la_step; //IMU_yaw_angle -= 1.0; |
| tnhnrl | 5:7421776f6b08 | 536 | PC.printf("- recieved\n"); |
| tnhnrl | 5:7421776f6b08 | 537 | PC.printf("LA auto step size: %f\n", la_step); |
| tnhnrl | 5:7421776f6b08 | 538 | PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp); |
| mdavis30 | 4:3c22d85a94a8 | 539 | } |
| tnhnrl | 5:7421776f6b08 | 540 | else |
| mdavis30 | 4:3c22d85a94a8 | 541 | { |
| tnhnrl | 5:7421776f6b08 | 542 | PC.printf("ERROR: LA in manual mode!\n"); |
| mdavis30 | 4:3c22d85a94a8 | 543 | } |
| mdavis30 | 4:3c22d85a94a8 | 544 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 545 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 546 | else if (Key =='=' or Key == '+') |
| tnhnrl | 5:7421776f6b08 | 547 | { |
| tnhnrl | 5:7421776f6b08 | 548 | if (LA_auto == true) |
| tnhnrl | 5:7421776f6b08 | 549 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 550 | la_setPoint_temp += la_step; //IMU_yaw_angle += 1.0; |
| tnhnrl | 5:7421776f6b08 | 551 | PC.printf("+ recieved\n"); |
| tnhnrl | 5:7421776f6b08 | 552 | PC.printf("LA auto step size: %f\n", la_step); |
| tnhnrl | 5:7421776f6b08 | 553 | PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp); |
| tnhnrl | 5:7421776f6b08 | 554 | } |
| tnhnrl | 5:7421776f6b08 | 555 | else |
| tnhnrl | 5:7421776f6b08 | 556 | { |
| tnhnrl | 5:7421776f6b08 | 557 | PC.printf("ERROR: LA in manual mode!\n"); |
| tnhnrl | 5:7421776f6b08 | 558 | } |
| tnhnrl | 5:7421776f6b08 | 559 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 560 | |
| mdavis30 | 3:1257a7d2eb3a | 561 | else if (Key == 'A' or Key == 'a') |
| mdavis30 | 6:ce2cf7f4d7d5 | 562 | { |
| tnhnrl | 5:7421776f6b08 | 563 | if (LA_auto == true) |
| tnhnrl | 5:7421776f6b08 | 564 | { |
| tnhnrl | 5:7421776f6b08 | 565 | PC.printf("A recieved\n"); |
| tnhnrl | 5:7421776f6b08 | 566 | la_setPoint=la_setPoint_temp; |
| tnhnrl | 5:7421776f6b08 | 567 | PC.printf("LA angle now set to\nLA_A_SND: %f\n", la_setPoint); |
| tnhnrl | 5:7421776f6b08 | 568 | } |
| tnhnrl | 5:7421776f6b08 | 569 | else |
| tnhnrl | 5:7421776f6b08 | 570 | { |
| tnhnrl | 5:7421776f6b08 | 571 | PC.printf("ERROR: LA in manual mode!\n"); |
| tnhnrl | 5:7421776f6b08 | 572 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 573 | } |
| mdavis30 | 4:3c22d85a94a8 | 574 | |
| mdavis30 | 0:381a84fad08b | 575 | |
| mdavis30 | 4:3c22d85a94a8 | 576 | else if(Key == 'n' or Key == 'N') |
| mdavis30 | 6:ce2cf7f4d7d5 | 577 | { |
| tnhnrl | 5:7421776f6b08 | 578 | if (LA_auto == false) |
| tnhnrl | 5:7421776f6b08 | 579 | { |
| tnhnrl | 5:7421776f6b08 | 580 | PC.printf("N key pressed. \n"); |
| mdavis30 | 9:2dcfd6ce61ea | 581 | PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY().c_str()); |
| tnhnrl | 5:7421776f6b08 | 582 | } |
| tnhnrl | 5:7421776f6b08 | 583 | |
| tnhnrl | 5:7421776f6b08 | 584 | else |
| tnhnrl | 5:7421776f6b08 | 585 | { |
| tnhnrl | 5:7421776f6b08 | 586 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 587 | } |
| mdavis30 | 4:3c22d85a94a8 | 588 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 589 | |
| mdavis30 | 4:3c22d85a94a8 | 590 | else if(Key == 'm' or Key == 'M') |
| mdavis30 | 4:3c22d85a94a8 | 591 | { |
| tnhnrl | 5:7421776f6b08 | 592 | if (LA_auto == false) |
| tnhnrl | 5:7421776f6b08 | 593 | { |
| tnhnrl | 5:7421776f6b08 | 594 | PC.printf("M key pressed. \n"); |
| mdavis30 | 9:2dcfd6ce61ea | 595 | PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY().c_str()); |
| tnhnrl | 5:7421776f6b08 | 596 | } |
| tnhnrl | 5:7421776f6b08 | 597 | |
| tnhnrl | 5:7421776f6b08 | 598 | else |
| tnhnrl | 5:7421776f6b08 | 599 | { |
| tnhnrl | 5:7421776f6b08 | 600 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 601 | } |
| mdavis30 | 0:381a84fad08b | 602 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 603 | |
| mdavis30 | 4:3c22d85a94a8 | 604 | else if(Key == 'j' or Key == 'J') |
| mdavis30 | 4:3c22d85a94a8 | 605 | { |
| tnhnrl | 5:7421776f6b08 | 606 | if (LA_auto == false) |
| tnhnrl | 5:7421776f6b08 | 607 | { |
| tnhnrl | 5:7421776f6b08 | 608 | PC.printf("J key pressed. \n"); |
| mdavis30 | 9:2dcfd6ce61ea | 609 | PC.printf("%s\n", BLA_object.Keyboard_A().c_str()); |
| tnhnrl | 5:7421776f6b08 | 610 | } |
| tnhnrl | 5:7421776f6b08 | 611 | |
| tnhnrl | 5:7421776f6b08 | 612 | else |
| tnhnrl | 5:7421776f6b08 | 613 | { |
| tnhnrl | 5:7421776f6b08 | 614 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 615 | } |
| mdavis30 | 0:381a84fad08b | 616 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 617 | |
| mdavis30 | 4:3c22d85a94a8 | 618 | else if(Key == 'k' or Key == 'K') |
| mdavis30 | 4:3c22d85a94a8 | 619 | { |
| tnhnrl | 5:7421776f6b08 | 620 | if (LA_auto == false) |
| tnhnrl | 5:7421776f6b08 | 621 | { |
| tnhnrl | 5:7421776f6b08 | 622 | PC.printf("K key pressed. \n"); |
| mdavis30 | 9:2dcfd6ce61ea | 623 | PC.printf("%s\n", BLA_object.Keyboard_D().c_str()); |
| tnhnrl | 5:7421776f6b08 | 624 | } |
| tnhnrl | 5:7421776f6b08 | 625 | |
| tnhnrl | 5:7421776f6b08 | 626 | else |
| tnhnrl | 5:7421776f6b08 | 627 | { |
| tnhnrl | 5:7421776f6b08 | 628 | PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n"); |
| tnhnrl | 5:7421776f6b08 | 629 | } |
| tnhnrl | 5:7421776f6b08 | 630 | } |
| mdavis30 | 0:381a84fad08b | 631 | |
| mdavis30 | 0:381a84fad08b | 632 | else |
| mdavis30 | 0:381a84fad08b | 633 | { |
| mdavis30 | 0:381a84fad08b | 634 | PC.printf("\n%c received!\n", Key); |
| mdavis30 | 0:381a84fad08b | 635 | PC.printf("\nDoing nothing.\n"); |
| mdavis30 | 0:381a84fad08b | 636 | } |
| mdavis30 | 0:381a84fad08b | 637 | |
| mdavis30 | 0:381a84fad08b | 638 | wait_us(100); //for PC readable |
| mdavis30 | 6:ce2cf7f4d7d5 | 639 | //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 | 640 | //BLA_object.PID_velocity_control(la_setPoint, IMU_yaw_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| tnhnrl | 5:7421776f6b08 | 641 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 642 | //end of PC readBLE |
| mdavis30 | 6:ce2cf7f4d7d5 | 643 | |
| tnhnrl | 5:7421776f6b08 | 644 | //MC readable |
| tnhnrl | 5:7421776f6b08 | 645 | MC_readable_string = BLA_object.MC_readable_redux(); |
| tnhnrl | 5:7421776f6b08 | 646 | //PC.printf("CHECK_MC_readable:\n%s\n", MC_readable_string); |
| mdavis30 | 0:381a84fad08b | 647 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 648 | /* if (LA_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 649 | { |
| tnhnrl | 5:7421776f6b08 | 650 | if (!MC_readable_string.empty()) //if this string is empty |
| tnhnrl | 5:7421776f6b08 | 651 | { |
| tnhnrl | 5:7421776f6b08 | 652 | PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller |
| tnhnrl | 5:7421776f6b08 | 653 | } |
| tnhnrl | 5:7421776f6b08 | 654 | else |
| tnhnrl | 5:7421776f6b08 | 655 | { |
| tnhnrl | 5:7421776f6b08 | 656 | ; |
| tnhnrl | 5:7421776f6b08 | 657 | //PC.printf("NOTHING?\n"); |
| tnhnrl | 5:7421776f6b08 | 658 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 659 | }*/ |
| mdavis30 | 6:ce2cf7f4d7d5 | 660 | if (LA_auto==true) |
| tnhnrl | 5:7421776f6b08 | 661 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 662 | //PC.printf("LA_auto true\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 663 | //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 | 664 | BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 665 | //wait_us(100); //for PC readable (0.1 ms) |
| tnhnrl | 5:7421776f6b08 | 666 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 667 | else if (LA_auto == false) |
| mdavis30 | 6:ce2cf7f4d7d5 | 668 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 669 | //PC.printf("LA_auto false\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 670 | // if (!MC_readable_string.empty()) //if this string is empty |
| mdavis30 | 6:ce2cf7f4d7d5 | 671 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 672 | // PC.printf("%s\n", MC_readable_string); //get responses from the linear actuator motor controller |
| mdavis30 | 6:ce2cf7f4d7d5 | 673 | // } |
| mdavis30 | 6:ce2cf7f4d7d5 | 674 | // else |
| mdavis30 | 6:ce2cf7f4d7d5 | 675 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 676 | // ; |
| mdavis30 | 6:ce2cf7f4d7d5 | 677 | // //PC.printf("NOTHING?\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 678 | // } |
| mdavis30 | 4:3c22d85a94a8 | 679 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 680 | // while (count_while==0) |
| mdavis30 | 6:ce2cf7f4d7d5 | 681 | // { |
| mdavis30 | 6:ce2cf7f4d7d5 | 682 | //// PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled |
| mdavis30 | 6:ce2cf7f4d7d5 | 683 | //// PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 684 | //// wait(1); |
| mdavis30 | 6:ce2cf7f4d7d5 | 685 | //// PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 686 | //// wait(1); |
| mdavis30 | 6:ce2cf7f4d7d5 | 687 | //// PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 688 | //// count_while++; |
| mdavis30 | 6:ce2cf7f4d7d5 | 689 | // |
| mdavis30 | 6:ce2cf7f4d7d5 | 690 | // BLA_object.Keyboard_E(); //turn on motor |
| mdavis30 | 6:ce2cf7f4d7d5 | 691 | // BLA_object.velocity_only(0); //set the velocity to zero just in case |
| mdavis30 | 6:ce2cf7f4d7d5 | 692 | // PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 693 | // count_while++; |
| mdavis30 | 6:ce2cf7f4d7d5 | 694 | // } |
| mdavis30 | 4:3c22d85a94a8 | 695 | } |
| mdavis30 | 4:3c22d85a94a8 | 696 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 697 | |
| mdavis30 | 0:381a84fad08b | 698 | if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0)) |
| mdavis30 | 0:381a84fad08b | 699 | { |
| mdavis30 | 0:381a84fad08b | 700 | count ++; |
| mdavis30 | 0:381a84fad08b | 701 | //pc().printf("We have a small issue\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 702 | if(count==10) |
| mdavis30 | 6:ce2cf7f4d7d5 | 703 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 704 | pc().printf("Bad pot issue\n"); |
| mdavis30 | 6:ce2cf7f4d7d5 | 705 | //hBridge().stop(); |
| mdavis30 | 6:ce2cf7f4d7d5 | 706 | count = 0; |
| mdavis30 | 7:10d7fbac30ea | 707 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 708 | } |
| mdavis30 | 0:381a84fad08b | 709 | |
| mdavis30 | 0:381a84fad08b | 710 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 711 | else if ((5.0*ain.read())<1.0) |
| mdavis30 | 2:c3cb3ea3c9fa | 712 | { |
| mdavis30 | 6:ce2cf7f4d7d5 | 713 | pc().printf("Hit the limit switch??\n"); |
| mdavis30 | 2:c3cb3ea3c9fa | 714 | hBridge().stop(); |
| mdavis30 | 7:10d7fbac30ea | 715 | wait(1); |
| mdavis30 | 7:10d7fbac30ea | 716 | hBridge().reset(); |
| mdavis30 | 7:10d7fbac30ea | 717 | positionCmd= 375; |
| mdavis30 | 7:10d7fbac30ea | 718 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 7:10d7fbac30ea | 719 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 8:9b9431404db7 | 720 | wait(2); |
| mdavis30 | 2:c3cb3ea3c9fa | 721 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 722 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 723 | //string snaps |
| mdavis30 | 0:381a84fad08b | 724 | else if (pvf().getVelocity() > 100) |
| mdavis30 | 0:381a84fad08b | 725 | { |
| tnhnrl | 5:7421776f6b08 | 726 | PC.printf("DEBUG: String position? %f velocity? %f\n", pvf().getPosition(), pvf().getVelocity()); //DEBUG TROY |
| mdavis30 | 0:381a84fad08b | 727 | //hBridge().stop(); |
| tnhnrl | 5:7421776f6b08 | 728 | //PC.printf("PosVelFilter B.E. Velocity: %f\n", pvf().getVelocity()); |
| mdavis30 | 6:ce2cf7f4d7d5 | 729 | PC.printf("********** String broke? *********\n"); |
| mdavis30 | 4:3c22d85a94a8 | 730 | } |
| mdavis30 | 6:ce2cf7f4d7d5 | 731 | |
| mdavis30 | 6:ce2cf7f4d7d5 | 732 | } |
| mdavis30 | 0:381a84fad08b | 733 | } |