FSG
/
7_20_17_FSG_
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: BCEmotor Battery_Linear_Actuator_ Controller_ IMU_code_ LTC1298_7_14 MODSERIAL PosVelFilter_7_14 System_ mbed
Fork of
7_14_17_FSG_working
by 
Troy Holley
main.cpp@4:3c22d85a94a8, 2017-07-07 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jul 07 21:23:19 2017 +0000
- Revision:
- 4:3c22d85a94a8
- Parent:
- 3:1257a7d2eb3a
- Child:
- 5:7421776f6b08
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 | 0:381a84fad08b | 20 | |
| mdavis30 | 0:381a84fad08b | 21 | string IMU_STRING = ""; |
| mdavis30 | 0:381a84fad08b | 22 | |
| mdavis30 | 0:381a84fad08b | 23 | |
| mdavis30 | 0:381a84fad08b | 24 | DigitalOut led1(LED1); |
| mdavis30 | 0:381a84fad08b | 25 | DigitalOut led2(LED2); |
| mdavis30 | 0:381a84fad08b | 26 | DigitalOut led3(LED3); |
| mdavis30 | 0:381a84fad08b | 27 | DigitalOut led4(LED4); |
| mdavis30 | 0:381a84fad08b | 28 | |
| mdavis30 | 0:381a84fad08b | 29 | AnalogIn pressure_analog_in(A5); //Initialize pin20 (read is float value) |
| mdavis30 | 2:c3cb3ea3c9fa | 30 | AnalogIn ain(p18); |
| mdavis30 | 0:381a84fad08b | 31 | |
| mdavis30 | 0:381a84fad08b | 32 | /* ************ These tickers work independent of any while loops ********** */ |
| mdavis30 | 0:381a84fad08b | 33 | Ticker IMU_ticker; //ticker for printing IMU //https://developer.mbed.org/handbook/Ticker |
| mdavis30 | 0:381a84fad08b | 34 | Ticker BE_position_ticker; //probably delete soon |
| mdavis30 | 0:381a84fad08b | 35 | Ticker PRESSURE_ticker; |
| mdavis30 | 0:381a84fad08b | 36 | |
| mdavis30 | 0:381a84fad08b | 37 | Ticker BCE_ticker; //new 6/5/17 |
| mdavis30 | 3:1257a7d2eb3a | 38 | Ticker PID_ticker; //new 6/14/17 |
| mdavis30 | 3:1257a7d2eb3a | 39 | Ticker LA_ticker; //new 6/22/17 |
| mdavis30 | 3:1257a7d2eb3a | 40 | |
| mdavis30 | 0:381a84fad08b | 41 | float positionCmd = 250.0; |
| mdavis30 | 4:3c22d85a94a8 | 42 | |
| mdavis30 | 0:381a84fad08b | 43 | /* ************************************************************************* */ |
| mdavis30 | 0:381a84fad08b | 44 | |
| mdavis30 | 4:3c22d85a94a8 | 45 | float pi = 3.14159265359; |
| mdavis30 | 4:3c22d85a94a8 | 46 | |
| mdavis30 | 3:1257a7d2eb3a | 47 | /* PID LOOP STUFF */ |
| mdavis30 | 3:1257a7d2eb3a | 48 | float la_setPoint = 0.00; //the IMU pitch angle we want (setpoint) |
| mdavis30 | 3:1257a7d2eb3a | 49 | |
| mdavis30 | 3:1257a7d2eb3a | 50 | float la_P_gain = 1.0; |
| mdavis30 | 3:1257a7d2eb3a | 51 | float la_I_gain = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 52 | float la_D_gain = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 53 | /* PID LOOP STUFF */ |
| mdavis30 | 3:1257a7d2eb3a | 54 | |
| mdavis30 | 3:1257a7d2eb3a | 55 | float IMU_pitch_angle = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 56 | |
| mdavis30 | 3:1257a7d2eb3a | 57 | bool motor_retracting = false; |
| mdavis30 | 3:1257a7d2eb3a | 58 | bool motor_extending = false; |
| mdavis30 | 3:1257a7d2eb3a | 59 | |
| mdavis30 | 0:381a84fad08b | 60 | void IMU_ticking() |
| mdavis30 | 0:381a84fad08b | 61 | { |
| mdavis30 | 0:381a84fad08b | 62 | led1 = !led1; //flash the IMU LED |
| mdavis30 | 3:1257a7d2eb3a | 63 | |
| mdavis30 | 4:3c22d85a94a8 | 64 | //PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it |
| mdavis30 | 4:3c22d85a94a8 | 65 | // PC.printf("\t pitch angle... %f set pitch angle: %f\n", IMU_pitch_angle, la_setPoint); |
| mdavis30 | 0:381a84fad08b | 66 | } |
| mdavis30 | 0:381a84fad08b | 67 | |
| mdavis30 | 0:381a84fad08b | 68 | void PRESSURE_ticking() |
| mdavis30 | 0:381a84fad08b | 69 | { |
| mdavis30 | 4:3c22d85a94a8 | 70 | //PC.printf("pressure: %f mm \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845); //read the analog pin |
| mdavis30 | 3:1257a7d2eb3a | 71 | //this voltage has been checked and scaled properly (6/28/2017) |
| mdavis30 | 0:381a84fad08b | 72 | } |
| mdavis30 | 0:381a84fad08b | 73 | |
| mdavis30 | 0:381a84fad08b | 74 | void BCE_ticking() //new 6/5/17 |
| mdavis30 | 0:381a84fad08b | 75 | { |
| mdavis30 | 0:381a84fad08b | 76 | 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 | 77 | } |
| mdavis30 | 0:381a84fad08b | 78 | |
| mdavis30 | 0:381a84fad08b | 79 | int main() |
| mdavis30 | 0:381a84fad08b | 80 | { |
| mdavis30 | 3:1257a7d2eb3a | 81 | PC.baud(9600); //mbed to PC serial connection speed |
| mdavis30 | 0:381a84fad08b | 82 | //PC.baud(230400); |
| mdavis30 | 0:381a84fad08b | 83 | //got screwy when i changed it |
| mdavis30 | 0:381a84fad08b | 84 | hBridge().stop(); |
| mdavis30 | 3:1257a7d2eb3a | 85 | |
| mdavis30 | 3:1257a7d2eb3a | 86 | PC.printf("Linear Actuator PID Program Started 6/22/17\n"); |
| mdavis30 | 3:1257a7d2eb3a | 87 | |
| mdavis30 | 3:1257a7d2eb3a | 88 | systemTime().start(); //start the timer, needed for PID loop |
| mdavis30 | 3:1257a7d2eb3a | 89 | |
| mdavis30 | 3:1257a7d2eb3a | 90 | /* **************** Linear Actuator MOTOR CONTROLLER **************** */ |
| mdavis30 | 3:1257a7d2eb3a | 91 | Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class |
| mdavis30 | 3:1257a7d2eb3a | 92 | |
| mdavis30 | 3:1257a7d2eb3a | 93 | PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled |
| mdavis30 | 3:1257a7d2eb3a | 94 | |
| mdavis30 | 3:1257a7d2eb3a | 95 | PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor |
| mdavis30 | 3:1257a7d2eb3a | 96 | wait(1); |
| mdavis30 | 3:1257a7d2eb3a | 97 | PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor |
| mdavis30 | 3:1257a7d2eb3a | 98 | wait(1); |
| mdavis30 | 3:1257a7d2eb3a | 99 | |
| mdavis30 | 0:381a84fad08b | 100 | //setup and start the adc. This runs on a fixed interval and is interrupt driven |
| mdavis30 | 0:381a84fad08b | 101 | adc().initialize(); |
| mdavis30 | 0:381a84fad08b | 102 | adc().start(); |
| mdavis30 | 0:381a84fad08b | 103 | |
| mdavis30 | 0:381a84fad08b | 104 | //Initialize the position velocity filter. This will consume a couple of seconds for |
| mdavis30 | 0:381a84fad08b | 105 | //the filter to converge |
| mdavis30 | 0:381a84fad08b | 106 | pvf().init(); |
| mdavis30 | 0:381a84fad08b | 107 | |
| mdavis30 | 0:381a84fad08b | 108 | ////CHANGED TO GLOBAL VARIABLES |
| mdavis30 | 0:381a84fad08b | 109 | float motor_cmd = 0.0; |
| mdavis30 | 0:381a84fad08b | 110 | // float positionCmd = 250.0; |
| mdavis30 | 0:381a84fad08b | 111 | float P = 0.10; |
| mdavis30 | 0:381a84fad08b | 112 | float I = 0.00; |
| mdavis30 | 0:381a84fad08b | 113 | float D = 0.00; |
| mdavis30 | 0:381a84fad08b | 114 | float count = 0.0; |
| mdavis30 | 0:381a84fad08b | 115 | //char userInput; //from Trent's code? |
| mdavis30 | 0:381a84fad08b | 116 | |
| mdavis30 | 4:3c22d85a94a8 | 117 | float la_step = 1.0; |
| mdavis30 | 4:3c22d85a94a8 | 118 | float la_setPoint_temp = 0.0; |
| mdavis30 | 4:3c22d85a94a8 | 119 | |
| mdavis30 | 4:3c22d85a94a8 | 120 | bool BCE_auto = true; |
| mdavis30 | 4:3c22d85a94a8 | 121 | bool LA_auto = true; |
| mdavis30 | 4:3c22d85a94a8 | 122 | |
| mdavis30 | 4:3c22d85a94a8 | 123 | float bce_auto_step_raw = 1.0; |
| mdavis30 | 4:3c22d85a94a8 | 124 | float bce_auto_step_l; |
| mdavis30 | 4:3c22d85a94a8 | 125 | float convert = 10000; |
| mdavis30 | 4:3c22d85a94a8 | 126 | float bce_auto_step_ml = bce_auto_step_raw * convert; |
| mdavis30 | 4:3c22d85a94a8 | 127 | int bce_man_step = 1; |
| mdavis30 | 4:3c22d85a94a8 | 128 | float volume_bce = 90.0*convert; |
| mdavis30 | 4:3c22d85a94a8 | 129 | float positionCmd_temp; |
| mdavis30 | 4:3c22d85a94a8 | 130 | float ml_to_l= 0.000000001; |
| mdavis30 | 3:1257a7d2eb3a | 131 | |
| mdavis30 | 0:381a84fad08b | 132 | hBridge().run(motor_cmd); |
| mdavis30 | 0:381a84fad08b | 133 | |
| mdavis30 | 0:381a84fad08b | 134 | //set the intial gains for the position controller |
| mdavis30 | 0:381a84fad08b | 135 | posCon().setPgain(P); |
| mdavis30 | 0:381a84fad08b | 136 | posCon().setIgain(I); |
| mdavis30 | 0:381a84fad08b | 137 | posCon().setDgain(D); |
| mdavis30 | 0:381a84fad08b | 138 | //posCon().writeSetPoint(positionCmd); |
| mdavis30 | 0:381a84fad08b | 139 | |
| mdavis30 | 0:381a84fad08b | 140 | /* *************************** LED *************************** */ |
| mdavis30 | 0:381a84fad08b | 141 | led1 = 1; //initial values |
| mdavis30 | 0:381a84fad08b | 142 | led2 = 1; |
| mdavis30 | 0:381a84fad08b | 143 | led3 = 1; |
| mdavis30 | 0:381a84fad08b | 144 | led4 = 1; |
| mdavis30 | 0:381a84fad08b | 145 | /* *************************** LED *************************** */ |
| mdavis30 | 0:381a84fad08b | 146 | |
| mdavis30 | 0:381a84fad08b | 147 | PC.printf("Program Started 6/5/17\n"); |
| mdavis30 | 4:3c22d85a94a8 | 148 | int cases = 0; |
| mdavis30 | 4:3c22d85a94a8 | 149 | int count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 150 | //hBridge().reset(); |
| mdavis30 | 4:3c22d85a94a8 | 151 | PC.printf("\n```````````Linear Actuator in IMU controlled mode````````````\n\n"); |
| mdavis30 | 0:381a84fad08b | 152 | //PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n"); |
| mdavis30 | 0:381a84fad08b | 153 | |
| mdavis30 | 0:381a84fad08b | 154 | /* *************************** Potentiometer *************************** */ |
| mdavis30 | 4:3c22d85a94a8 | 155 | PRESSURE_ticker.attach(&PRESSURE_ticking, 3.0); |
| mdavis30 | 0:381a84fad08b | 156 | /* *************************** Potentiometer *************************** */ |
| mdavis30 | 0:381a84fad08b | 157 | |
| mdavis30 | 0:381a84fad08b | 158 | /* *************************** MOTOR CONTROLLER *************************** */ |
| mdavis30 | 3:1257a7d2eb3a | 159 | //Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class |
| mdavis30 | 0:381a84fad08b | 160 | /* *************************** MOTOR CONTROLLER *************************** */ |
| mdavis30 | 0:381a84fad08b | 161 | |
| mdavis30 | 0:381a84fad08b | 162 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 163 | IMU_code IMU_object; //create the IMU object from the imported class |
| mdavis30 | 4:3c22d85a94a8 | 164 | IMU_ticker.attach(&IMU_ticking, 3.0); |
| mdavis30 | 0:381a84fad08b | 165 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 166 | |
| mdavis30 | 0:381a84fad08b | 167 | /* *************************** BCE *************************** */ |
| mdavis30 | 3:1257a7d2eb3a | 168 | //float previous_positionCmd = -1; |
| mdavis30 | 4:3c22d85a94a8 | 169 | BCE_ticker.attach(&BCE_ticking, 3.0); |
| mdavis30 | 0:381a84fad08b | 170 | /* *************************** BCE *************************** */ |
| mdavis30 | 0:381a84fad08b | 171 | |
| mdavis30 | 0:381a84fad08b | 172 | while(1) |
| mdavis30 | 0:381a84fad08b | 173 | { |
| mdavis30 | 0:381a84fad08b | 174 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 175 | IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop |
| mdavis30 | 3:1257a7d2eb3a | 176 | IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly? |
| mdavis30 | 3:1257a7d2eb3a | 177 | //PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_pitch_angle, la_setPoint); |
| mdavis30 | 0:381a84fad08b | 178 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 179 | |
| mdavis30 | 0:381a84fad08b | 180 | /* Buoyancy Engine */ |
| mdavis30 | 0:381a84fad08b | 181 | // update the position velocity filter |
| mdavis30 | 0:381a84fad08b | 182 | pvf().update(); |
| mdavis30 | 0:381a84fad08b | 183 | |
| mdavis30 | 0:381a84fad08b | 184 | //update the controller with the current numbers in the position guesser |
| mdavis30 | 0:381a84fad08b | 185 | posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ; |
| mdavis30 | 0:381a84fad08b | 186 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 0:381a84fad08b | 187 | |
| mdavis30 | 0:381a84fad08b | 188 | /* Buoyancy Engine */ |
| mdavis30 | 0:381a84fad08b | 189 | |
| mdavis30 | 0:381a84fad08b | 190 | //FOR DEBUGGING |
| mdavis30 | 0:381a84fad08b | 191 | //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 | 192 | |
| mdavis30 | 0:381a84fad08b | 193 | //PC.printf("WHILE LOOP\n"); //DEBUG |
| mdavis30 | 0:381a84fad08b | 194 | if (PC.readable()) |
| mdavis30 | 0:381a84fad08b | 195 | { |
| mdavis30 | 3:1257a7d2eb3a | 196 | //led4 != led4; |
| mdavis30 | 3:1257a7d2eb3a | 197 | //PC.printf("DEBUG: PC IS READABLE\n"); //DEBUG |
| mdavis30 | 0:381a84fad08b | 198 | |
| mdavis30 | 4:3c22d85a94a8 | 199 | Key=PC.getc(); |
| mdavis30 | 4:3c22d85a94a8 | 200 | //Universal MBED Controls |
| mdavis30 | 4:3c22d85a94a8 | 201 | if(Key=='!') //RESET THE MBED |
| mdavis30 | 0:381a84fad08b | 202 | { |
| mdavis30 | 4:3c22d85a94a8 | 203 | PC.printf("MBED RESET KEY (!) PRESSED\n"); |
| mdavis30 | 4:3c22d85a94a8 | 204 | PC.printf("Linear Actuator Motor disabled!\n"); |
| mdavis30 | 4:3c22d85a94a8 | 205 | //disable the motor |
| mdavis30 | 4:3c22d85a94a8 | 206 | BLA_object.Keyboard_Q(); //DISABLE THE MOTOR |
| mdavis30 | 4:3c22d85a94a8 | 207 | wait(0.5); //500 milliseconds |
| mdavis30 | 4:3c22d85a94a8 | 208 | mbed_reset(); //reset the mbed! |
| mdavis30 | 0:381a84fad08b | 209 | } |
| mdavis30 | 4:3c22d85a94a8 | 210 | else if(Key =='H') //homing sequence |
| mdavis30 | 0:381a84fad08b | 211 | { |
| mdavis30 | 4:3c22d85a94a8 | 212 | PC.printf("### homing the device ###"); |
| mdavis30 | 4:3c22d85a94a8 | 213 | BLA_object.Keyboard_H(); |
| mdavis30 | 4:3c22d85a94a8 | 214 | wait(5); //for debugging |
| mdavis30 | 4:3c22d85a94a8 | 215 | |
| mdavis30 | 4:3c22d85a94a8 | 216 | PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str()); //flip this back and forth |
| mdavis30 | 4:3c22d85a94a8 | 217 | wait(1); //for debugging |
| mdavis30 | 4:3c22d85a94a8 | 218 | } |
| mdavis30 | 3:1257a7d2eb3a | 219 | else if(Key=='p' or Key == 'P') |
| mdavis30 | 3:1257a7d2eb3a | 220 | { |
| mdavis30 | 3:1257a7d2eb3a | 221 | PC.printf("### position is %s ###\n", BLA_object.get_pos().c_str()); //flip this back and forth |
| mdavis30 | 4:3c22d85a94a8 | 222 | wait(1); //for debugging |
| mdavis30 | 3:1257a7d2eb3a | 223 | // "-999999" means it is not working |
| mdavis30 | 3:1257a7d2eb3a | 224 | } |
| mdavis30 | 4:3c22d85a94a8 | 225 | |
| mdavis30 | 4:3c22d85a94a8 | 226 | //Buoyancy Engine Controls |
| mdavis30 | 4:3c22d85a94a8 | 227 | else if (Key == ',' or Key == '<') |
| mdavis30 | 4:3c22d85a94a8 | 228 | { |
| mdavis30 | 4:3c22d85a94a8 | 229 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 230 | { |
| mdavis30 | 4:3c22d85a94a8 | 231 | PC.printf("BCE: Now in Automatic Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 232 | BCE_auto = true; |
| mdavis30 | 4:3c22d85a94a8 | 233 | } |
| mdavis30 | 4:3c22d85a94a8 | 234 | else |
| mdavis30 | 4:3c22d85a94a8 | 235 | { |
| mdavis30 | 4:3c22d85a94a8 | 236 | PC.printf("BCE: Still in Manual Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 237 | } |
| mdavis30 | 4:3c22d85a94a8 | 238 | } |
| mdavis30 | 4:3c22d85a94a8 | 239 | else if (Key == '.' or Key == '>') |
| mdavis30 | 4:3c22d85a94a8 | 240 | { |
| mdavis30 | 4:3c22d85a94a8 | 241 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 242 | { |
| mdavis30 | 4:3c22d85a94a8 | 243 | PC.printf("BCE: Now in Manual Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 244 | BCE_auto = false; |
| mdavis30 | 4:3c22d85a94a8 | 245 | } |
| mdavis30 | 4:3c22d85a94a8 | 246 | else |
| mdavis30 | 4:3c22d85a94a8 | 247 | { |
| mdavis30 | 4:3c22d85a94a8 | 248 | PC.printf("BCE: Still in Automatic Mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 249 | } |
| mdavis30 | 4:3c22d85a94a8 | 250 | } |
| mdavis30 | 4:3c22d85a94a8 | 251 | //BCE Automatic Controls |
| mdavis30 | 4:3c22d85a94a8 | 252 | else if (Key == 's' or Key == 'S') |
| mdavis30 | 4:3c22d85a94a8 | 253 | { |
| mdavis30 | 4:3c22d85a94a8 | 254 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 255 | { |
| mdavis30 | 4:3c22d85a94a8 | 256 | //PC.printf("BCE Automatic Step Size Change\n"); |
| mdavis30 | 4:3c22d85a94a8 | 257 | if (bce_auto_step_raw == 1.0) |
| mdavis30 | 4:3c22d85a94a8 | 258 | { |
| mdavis30 | 4:3c22d85a94a8 | 259 | bce_auto_step_raw = 5.0; |
| mdavis30 | 4:3c22d85a94a8 | 260 | } |
| mdavis30 | 4:3c22d85a94a8 | 261 | else if (bce_auto_step_raw == 5.0) |
| mdavis30 | 4:3c22d85a94a8 | 262 | { |
| mdavis30 | 4:3c22d85a94a8 | 263 | bce_auto_step_raw = 10.0; |
| mdavis30 | 4:3c22d85a94a8 | 264 | } |
| mdavis30 | 4:3c22d85a94a8 | 265 | else if (bce_auto_step_raw == 10.0) |
| mdavis30 | 4:3c22d85a94a8 | 266 | { |
| mdavis30 | 4:3c22d85a94a8 | 267 | bce_auto_step_raw = 1.0; |
| mdavis30 | 4:3c22d85a94a8 | 268 | } |
| mdavis30 | 4:3c22d85a94a8 | 269 | bce_auto_step_ml = bce_auto_step_raw * convert; |
| mdavis30 | 4:3c22d85a94a8 | 270 | PC.printf("BCE Auto Step Size Now %7.0f milliliters\n", bce_auto_step_ml); |
| mdavis30 | 4:3c22d85a94a8 | 271 | } |
| mdavis30 | 4:3c22d85a94a8 | 272 | else |
| mdavis30 | 4:3c22d85a94a8 | 273 | { |
| mdavis30 | 4:3c22d85a94a8 | 274 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 275 | |
| mdavis30 | 4:3c22d85a94a8 | 276 | } |
| mdavis30 | 4:3c22d85a94a8 | 277 | } |
| mdavis30 | 4:3c22d85a94a8 | 278 | else if(Key =='d' or Key == 'D') |
| mdavis30 | 4:3c22d85a94a8 | 279 | { |
| mdavis30 | 4:3c22d85a94a8 | 280 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 281 | { |
| mdavis30 | 4:3c22d85a94a8 | 282 | volume_bce -= bce_auto_step_ml; |
| mdavis30 | 4:3c22d85a94a8 | 283 | PC.printf("The volume for the buoyancy motor is\nVBE: %1.5f liters\n", volume_bce*ml_to_l); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 284 | } |
| mdavis30 | 4:3c22d85a94a8 | 285 | else |
| mdavis30 | 4:3c22d85a94a8 | 286 | { |
| mdavis30 | 4:3c22d85a94a8 | 287 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 288 | } |
| mdavis30 | 4:3c22d85a94a8 | 289 | } |
| mdavis30 | 4:3c22d85a94a8 | 290 | else if(Key == 'f' or Key == 'F') |
| mdavis30 | 4:3c22d85a94a8 | 291 | { |
| mdavis30 | 4:3c22d85a94a8 | 292 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 293 | { |
| mdavis30 | 4:3c22d85a94a8 | 294 | volume_bce += bce_auto_step_ml; |
| mdavis30 | 4:3c22d85a94a8 | 295 | PC.printf("The volume for the buoyancy motor is\nVBE: %1.5f liters\n", volume_bce*ml_to_l); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 296 | } |
| mdavis30 | 4:3c22d85a94a8 | 297 | else |
| mdavis30 | 4:3c22d85a94a8 | 298 | { |
| mdavis30 | 4:3c22d85a94a8 | 299 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 300 | } |
| mdavis30 | 4:3c22d85a94a8 | 301 | } |
| mdavis30 | 4:3c22d85a94a8 | 302 | else if(Key == 'r' or Key == 'R') |
| mdavis30 | 4:3c22d85a94a8 | 303 | { |
| mdavis30 | 4:3c22d85a94a8 | 304 | if (BCE_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 305 | { |
| mdavis30 | 4:3c22d85a94a8 | 306 | PC.printf("\nR received!\n"); |
| mdavis30 | 4:3c22d85a94a8 | 307 | positionCmd=(volume_bce-(pi*40.64*40.64*377))/(-1*pi*40.64*40.64); |
| mdavis30 | 4:3c22d85a94a8 | 308 | //positionCmd= positionCmd_temp*0.000000001; |
| mdavis30 | 4:3c22d85a94a8 | 309 | //PC.printf("BCE engine going to position: %3.2f\n", positionCmd); |
| mdavis30 | 4:3c22d85a94a8 | 310 | PC.printf("\nBASETP: %3.0f\n", positionCmd); |
| mdavis30 | 4:3c22d85a94a8 | 311 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 4:3c22d85a94a8 | 312 | //posCon().setPgain(P); |
| mdavis30 | 4:3c22d85a94a8 | 313 | //posCon().setIgain(I); |
| mdavis30 | 4:3c22d85a94a8 | 314 | //posCon().setDgain(D); |
| mdavis30 | 4:3c22d85a94a8 | 315 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 4:3c22d85a94a8 | 316 | |
| mdavis30 | 4:3c22d85a94a8 | 317 | hBridge().reset(); |
| mdavis30 | 4:3c22d85a94a8 | 318 | |
| mdavis30 | 4:3c22d85a94a8 | 319 | count = 0; |
| mdavis30 | 4:3c22d85a94a8 | 320 | } |
| mdavis30 | 4:3c22d85a94a8 | 321 | else |
| mdavis30 | 4:3c22d85a94a8 | 322 | { |
| mdavis30 | 4:3c22d85a94a8 | 323 | PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 324 | } |
| mdavis30 | 4:3c22d85a94a8 | 325 | } |
| mdavis30 | 4:3c22d85a94a8 | 326 | //BCE Manual Controls |
| mdavis30 | 4:3c22d85a94a8 | 327 | else if (Key == '2' or Key == '@') |
| mdavis30 | 4:3c22d85a94a8 | 328 | { |
| mdavis30 | 4:3c22d85a94a8 | 329 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 330 | { |
| mdavis30 | 4:3c22d85a94a8 | 331 | PC.printf("BCE Manual Step Size Change\n"); |
| mdavis30 | 4:3c22d85a94a8 | 332 | if (bce_man_step == 1) |
| mdavis30 | 4:3c22d85a94a8 | 333 | { |
| mdavis30 | 4:3c22d85a94a8 | 334 | bce_man_step = 10; |
| mdavis30 | 4:3c22d85a94a8 | 335 | } |
| mdavis30 | 4:3c22d85a94a8 | 336 | else if (bce_man_step == 10) |
| mdavis30 | 4:3c22d85a94a8 | 337 | { |
| mdavis30 | 4:3c22d85a94a8 | 338 | bce_man_step = 25; |
| mdavis30 | 4:3c22d85a94a8 | 339 | } |
| mdavis30 | 4:3c22d85a94a8 | 340 | else if (bce_man_step == 25) |
| mdavis30 | 4:3c22d85a94a8 | 341 | { |
| mdavis30 | 4:3c22d85a94a8 | 342 | bce_man_step = 50; |
| mdavis30 | 4:3c22d85a94a8 | 343 | } |
| mdavis30 | 4:3c22d85a94a8 | 344 | else if (bce_man_step == 50) |
| mdavis30 | 4:3c22d85a94a8 | 345 | { |
| mdavis30 | 4:3c22d85a94a8 | 346 | bce_man_step = 1; |
| mdavis30 | 4:3c22d85a94a8 | 347 | } |
| mdavis30 | 4:3c22d85a94a8 | 348 | |
| mdavis30 | 4:3c22d85a94a8 | 349 | PC.printf("BCE Manual Step Size Now %d\n", bce_man_step); |
| mdavis30 | 4:3c22d85a94a8 | 350 | } |
| mdavis30 | 4:3c22d85a94a8 | 351 | else |
| mdavis30 | 4:3c22d85a94a8 | 352 | { |
| mdavis30 | 4:3c22d85a94a8 | 353 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 354 | } |
| mdavis30 | 4:3c22d85a94a8 | 355 | } |
| mdavis30 | 4:3c22d85a94a8 | 356 | else if (Key == 'z' or Key =='Z') |
| mdavis30 | 4:3c22d85a94a8 | 357 | { |
| mdavis30 | 4:3c22d85a94a8 | 358 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 359 | { |
| mdavis30 | 4:3c22d85a94a8 | 360 | //increment the duty cycle |
| mdavis30 | 4:3c22d85a94a8 | 361 | positionCmd -= bce_man_step; |
| mdavis30 | 4:3c22d85a94a8 | 362 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 363 | } |
| mdavis30 | 4:3c22d85a94a8 | 364 | else |
| mdavis30 | 4:3c22d85a94a8 | 365 | { |
| mdavis30 | 4:3c22d85a94a8 | 366 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 367 | } |
| mdavis30 | 4:3c22d85a94a8 | 368 | } |
| mdavis30 | 4:3c22d85a94a8 | 369 | else if (Key == 'x' or Key == 'X') |
| mdavis30 | 4:3c22d85a94a8 | 370 | { |
| mdavis30 | 4:3c22d85a94a8 | 371 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 372 | { |
| mdavis30 | 4:3c22d85a94a8 | 373 | //decrement the duty cycle |
| mdavis30 | 4:3c22d85a94a8 | 374 | positionCmd += bce_man_step; |
| mdavis30 | 4:3c22d85a94a8 | 375 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 4:3c22d85a94a8 | 376 | } |
| mdavis30 | 4:3c22d85a94a8 | 377 | else |
| mdavis30 | 4:3c22d85a94a8 | 378 | { |
| mdavis30 | 4:3c22d85a94a8 | 379 | PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n"); |
| mdavis30 | 4:3c22d85a94a8 | 380 | } |
| mdavis30 | 4:3c22d85a94a8 | 381 | } |
| mdavis30 | 4:3c22d85a94a8 | 382 | else if(Key=='w' or Key =='W') |
| mdavis30 | 4:3c22d85a94a8 | 383 | { |
| mdavis30 | 4:3c22d85a94a8 | 384 | if (BCE_auto == false) |
| mdavis30 | 4:3c22d85a94a8 | 385 | { |
| mdavis30 | 4:3c22d85a94a8 | 386 | PC.printf("\nW received!\n"); |
| mdavis30 | 4:3c22d85a94a8 | 387 | PC.printf("BASETP: %3.0f\n", positionCmd); |
| mdavis30 | 4:3c22d85a94a8 | 388 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 4:3c22d85a94a8 | 389 | //posCon().setPgain(P); |
| mdavis30 | 4:3c22d85a94a8 | 390 | //posCon().setIgain(I); |
| mdavis30 | 4:3c22d85a94a8 | 391 | //posCon().setDgain(D); |
| mdavis30 | 4:3c22d85a94a8 | 392 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 4:3c22d85a94a8 | 393 | |
| mdavis30 | 4:3c22d85a94a8 | 394 | hBridge().reset(); |
| mdavis30 | 4:3c22d85a94a8 | 395 | |
| mdavis30 | 4:3c22d85a94a8 | 396 | count = 0; |
| mdavis30 | 4:3c22d85a94a8 | 397 | |
| 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 | 4:3c22d85a94a8 | 404 | |
| mdavis30 | 4:3c22d85a94a8 | 405 | //Linear Actuator Controls |
| mdavis30 | 4:3c22d85a94a8 | 406 | else if(Key == 'c' or Key == 'C') |
| mdavis30 | 4:3c22d85a94a8 | 407 | { |
| mdavis30 | 4:3c22d85a94a8 | 408 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 409 | { |
| mdavis30 | 4:3c22d85a94a8 | 410 | PC.printf("ERROR: LA already in auto mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 411 | } |
| mdavis30 | 4:3c22d85a94a8 | 412 | else |
| mdavis30 | 4:3c22d85a94a8 | 413 | { |
| mdavis30 | 4:3c22d85a94a8 | 414 | LA_auto = true; |
| mdavis30 | 4:3c22d85a94a8 | 415 | PC.printf("```````````Now in IMU Controlled Mode```````````````\n"); |
| mdavis30 | 4:3c22d85a94a8 | 416 | cases = 0; |
| mdavis30 | 4:3c22d85a94a8 | 417 | count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 418 | } |
| mdavis30 | 4:3c22d85a94a8 | 419 | } |
| mdavis30 | 4:3c22d85a94a8 | 420 | else if (Key == 'v' or Key == 'V') |
| mdavis30 | 4:3c22d85a94a8 | 421 | { |
| mdavis30 | 4:3c22d85a94a8 | 422 | if (LA_auto == true) |
| mdavis30 | 4:3c22d85a94a8 | 423 | { |
| mdavis30 | 4:3c22d85a94a8 | 424 | LA_auto = false; |
| mdavis30 | 4:3c22d85a94a8 | 425 | //Change cases: go from imu controlled to manual |
| mdavis30 | 4:3c22d85a94a8 | 426 | PC.printf("```````````Now in Manual Mode````````````````````\n"); |
| mdavis30 | 4:3c22d85a94a8 | 427 | cases = 1; |
| mdavis30 | 4:3c22d85a94a8 | 428 | count_while = 0; |
| mdavis30 | 4:3c22d85a94a8 | 429 | } |
| mdavis30 | 4:3c22d85a94a8 | 430 | else |
| mdavis30 | 4:3c22d85a94a8 | 431 | { |
| mdavis30 | 4:3c22d85a94a8 | 432 | PC.printf("ERROR: LA already in manual mode\n"); |
| mdavis30 | 4:3c22d85a94a8 | 433 | } |
| mdavis30 | 4:3c22d85a94a8 | 434 | } |
| mdavis30 | 4:3c22d85a94a8 | 435 | else if (Key == '0' or Key == ')') |
| mdavis30 | 4:3c22d85a94a8 | 436 | { |
| mdavis30 | 4:3c22d85a94a8 | 437 | PC.printf(") recieved\n"); |
| mdavis30 | 4:3c22d85a94a8 | 438 | if (la_step == 0.5) |
| mdavis30 | 4:3c22d85a94a8 | 439 | { |
| mdavis30 | 4:3c22d85a94a8 | 440 | la_step = 1.0; |
| mdavis30 | 4:3c22d85a94a8 | 441 | } |
| mdavis30 | 4:3c22d85a94a8 | 442 | else if (la_step == 1.0) |
| mdavis30 | 4:3c22d85a94a8 | 443 | { |
| mdavis30 | 4:3c22d85a94a8 | 444 | la_step = 5.0; |
| mdavis30 | 4:3c22d85a94a8 | 445 | } |
| mdavis30 | 4:3c22d85a94a8 | 446 | else if (la_step == 5.0) |
| mdavis30 | 4:3c22d85a94a8 | 447 | { |
| mdavis30 | 4:3c22d85a94a8 | 448 | la_step = 10.0; |
| mdavis30 | 4:3c22d85a94a8 | 449 | } |
| mdavis30 | 4:3c22d85a94a8 | 450 | else if (la_step == 10.0) |
| mdavis30 | 4:3c22d85a94a8 | 451 | { |
| mdavis30 | 4:3c22d85a94a8 | 452 | la_step = 15.0; |
| mdavis30 | 4:3c22d85a94a8 | 453 | } |
| mdavis30 | 4:3c22d85a94a8 | 454 | else if (la_step == 15.0) |
| mdavis30 | 4:3c22d85a94a8 | 455 | { |
| mdavis30 | 4:3c22d85a94a8 | 456 | la_step = 0.5; |
| mdavis30 | 4:3c22d85a94a8 | 457 | } |
| mdavis30 | 4:3c22d85a94a8 | 458 | PC.printf("LA Step Size Now %f\n", la_step); |
| mdavis30 | 4:3c22d85a94a8 | 459 | } |
| mdavis30 | 4:3c22d85a94a8 | 460 | |
| mdavis30 | 4:3c22d85a94a8 | 461 | |
| mdavis30 | 3:1257a7d2eb3a | 462 | else if (Key == 'A' or Key == 'a') |
| mdavis30 | 0:381a84fad08b | 463 | { |
| mdavis30 | 3:1257a7d2eb3a | 464 | PC.printf("A recieved\n"); |
| mdavis30 | 3:1257a7d2eb3a | 465 | la_setPoint=la_setPoint_temp; |
| mdavis30 | 3:1257a7d2eb3a | 466 | PC.printf("LA angle now set to %f\n", la_setPoint); |
| mdavis30 | 3:1257a7d2eb3a | 467 | } |
| mdavis30 | 3:1257a7d2eb3a | 468 | else if (Key=='-' or Key == '_') |
| mdavis30 | 0:381a84fad08b | 469 | { |
| mdavis30 | 3:1257a7d2eb3a | 470 | la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0; |
| mdavis30 | 3:1257a7d2eb3a | 471 | PC.printf("- recieved\n"); |
| mdavis30 | 4:3c22d85a94a8 | 472 | PC.printf("LA angle changed to: %d la_step: %f\n", la_setPoint_temp, la_step); |
| mdavis30 | 3:1257a7d2eb3a | 473 | } |
| mdavis30 | 3:1257a7d2eb3a | 474 | else if (Key =='=' or Key == '+') |
| mdavis30 | 3:1257a7d2eb3a | 475 | { |
| mdavis30 | 3:1257a7d2eb3a | 476 | la_setPoint_temp += la_step; //IMU_pitch_angle += 1.0; |
| mdavis30 | 3:1257a7d2eb3a | 477 | PC.printf("+ recieved\n"); |
| mdavis30 | 4:3c22d85a94a8 | 478 | PC.printf("LA angle changed to: %f\n", la_setPoint_temp); |
| mdavis30 | 0:381a84fad08b | 479 | } |
| mdavis30 | 4:3c22d85a94a8 | 480 | |
| mdavis30 | 3:1257a7d2eb3a | 481 | else if (Key=='[' or Key == '{') |
| mdavis30 | 3:1257a7d2eb3a | 482 | { |
| mdavis30 | 3:1257a7d2eb3a | 483 | la_P_gain -= 0.1; |
| mdavis30 | 3:1257a7d2eb3a | 484 | PC.printf("[ key pressed\n"); |
| mdavis30 | 3:1257a7d2eb3a | 485 | PC.printf("P gain is now %f\n", la_P_gain); |
| mdavis30 | 3:1257a7d2eb3a | 486 | |
| mdavis30 | 3:1257a7d2eb3a | 487 | } |
| mdavis30 | 3:1257a7d2eb3a | 488 | else if (Key==']' or Key == '}') |
| mdavis30 | 3:1257a7d2eb3a | 489 | { |
| mdavis30 | 3:1257a7d2eb3a | 490 | la_P_gain += 0.1; |
| mdavis30 | 3:1257a7d2eb3a | 491 | PC.printf("] key pressed\n"); |
| mdavis30 | 3:1257a7d2eb3a | 492 | PC.printf("P gain is now %f\n", la_P_gain); |
| mdavis30 | 3:1257a7d2eb3a | 493 | |
| mdavis30 | 3:1257a7d2eb3a | 494 | } |
| mdavis30 | 3:1257a7d2eb3a | 495 | else if (Key==';') |
| mdavis30 | 3:1257a7d2eb3a | 496 | { |
| mdavis30 | 3:1257a7d2eb3a | 497 | la_I_gain -= 0.1; |
| mdavis30 | 4:3c22d85a94a8 | 498 | PC.printf("; key pressed\n"); |
| mdavis30 | 4:3c22d85a94a8 | 499 | PC.printf("I gain is now %f\n", la_I_gain); |
| mdavis30 | 3:1257a7d2eb3a | 500 | |
| mdavis30 | 0:381a84fad08b | 501 | |
| mdavis30 | 0:381a84fad08b | 502 | } |
| mdavis30 | 3:1257a7d2eb3a | 503 | else if (Key=='\'') |
| mdavis30 | 4:3c22d85a94a8 | 504 | { |
| mdavis30 | 3:1257a7d2eb3a | 505 | la_I_gain += 0.1; |
| mdavis30 | 4:3c22d85a94a8 | 506 | PC.printf("\ key pressed\n"); |
| mdavis30 | 4:3c22d85a94a8 | 507 | PC.printf("I gain is now %f\n", la_I_gain); |
| mdavis30 | 4:3c22d85a94a8 | 508 | } |
| mdavis30 | 3:1257a7d2eb3a | 509 | else if (Key=='.') |
| mdavis30 | 0:381a84fad08b | 510 | { |
| mdavis30 | 4:3c22d85a94a8 | 511 | la_D_gain -= 0.1; |
| mdavis30 | 4:3c22d85a94a8 | 512 | PC.printf(". key pressed\n"); |
| mdavis30 | 4:3c22d85a94a8 | 513 | PC.printf("D gain is now %f\n", la_D_gain); |
| mdavis30 | 4:3c22d85a94a8 | 514 | } |
| mdavis30 | 4:3c22d85a94a8 | 515 | else if (Key=='/') |
| mdavis30 | 4:3c22d85a94a8 | 516 | { |
| mdavis30 | 4:3c22d85a94a8 | 517 | la_D_gain += 0.1; |
| mdavis30 | 4:3c22d85a94a8 | 518 | PC.printf("/ key pressed\n"); |
| mdavis30 | 4:3c22d85a94a8 | 519 | PC.printf("D gain is now %f\n", la_D_gain); |
| mdavis30 | 4:3c22d85a94a8 | 520 | } |
| mdavis30 | 4:3c22d85a94a8 | 521 | |
| mdavis30 | 4:3c22d85a94a8 | 522 | else if(Key == 'n' or Key == 'N') |
| mdavis30 | 0:381a84fad08b | 523 | { |
| mdavis30 | 4:3c22d85a94a8 | 524 | PC.printf("N key pressed. \n"); |
| mdavis30 | 4:3c22d85a94a8 | 525 | PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY()); |
| mdavis30 | 4:3c22d85a94a8 | 526 | } |
| mdavis30 | 4:3c22d85a94a8 | 527 | else if(Key == 'm' or Key == 'M') |
| mdavis30 | 4:3c22d85a94a8 | 528 | { |
| mdavis30 | 4:3c22d85a94a8 | 529 | PC.printf("M key pressed. \n"); |
| mdavis30 | 4:3c22d85a94a8 | 530 | PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY()); |
| mdavis30 | 0:381a84fad08b | 531 | } |
| mdavis30 | 4:3c22d85a94a8 | 532 | else if(Key == 'j' or Key == 'J') |
| mdavis30 | 4:3c22d85a94a8 | 533 | { |
| mdavis30 | 4:3c22d85a94a8 | 534 | PC.printf("J key pressed. \n"); |
| mdavis30 | 4:3c22d85a94a8 | 535 | PC.printf("%s\n", BLA_object.Keyboard_A()); |
| mdavis30 | 0:381a84fad08b | 536 | } |
| mdavis30 | 4:3c22d85a94a8 | 537 | else if(Key == 'k' or Key == 'K') |
| mdavis30 | 4:3c22d85a94a8 | 538 | { |
| mdavis30 | 4:3c22d85a94a8 | 539 | PC.printf("K key pressed. \n"); |
| mdavis30 | 4:3c22d85a94a8 | 540 | PC.printf("%s\n", BLA_object.Keyboard_D()); |
| mdavis30 | 4:3c22d85a94a8 | 541 | } |
| mdavis30 | 4:3c22d85a94a8 | 542 | |
| mdavis30 | 0:381a84fad08b | 543 | |
| mdavis30 | 0:381a84fad08b | 544 | else |
| mdavis30 | 0:381a84fad08b | 545 | { |
| mdavis30 | 0:381a84fad08b | 546 | PC.printf("\n%c received!\n", Key); |
| mdavis30 | 0:381a84fad08b | 547 | PC.printf("\nDoing nothing.\n"); |
| mdavis30 | 0:381a84fad08b | 548 | } |
| mdavis30 | 0:381a84fad08b | 549 | |
| mdavis30 | 0:381a84fad08b | 550 | wait_us(100); //for PC readable |
| mdavis30 | 3:1257a7d2eb3a | 551 | //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str()); //get output string |
| mdavis30 | 3:1257a7d2eb3a | 552 | //BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| mdavis30 | 3:1257a7d2eb3a | 553 | } |
| mdavis30 | 0:381a84fad08b | 554 | |
| mdavis30 | 4:3c22d85a94a8 | 555 | if (cases==0) |
| mdavis30 | 4:3c22d85a94a8 | 556 | { |
| mdavis30 | 4:3c22d85a94a8 | 557 | //PC.printf("Case is 0; IMU control\n"); |
| mdavis30 | 4:3c22d85a94a8 | 558 | //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 | 4:3c22d85a94a8 | 559 | BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| mdavis30 | 4:3c22d85a94a8 | 560 | //wait_us(100); //for PC readable (0.1 ms) |
| mdavis30 | 4:3c22d85a94a8 | 561 | } |
| mdavis30 | 4:3c22d85a94a8 | 562 | else if (cases==1) |
| mdavis30 | 4:3c22d85a94a8 | 563 | { |
| mdavis30 | 4:3c22d85a94a8 | 564 | |
| mdavis30 | 4:3c22d85a94a8 | 565 | while (count_while==0) |
| mdavis30 | 4:3c22d85a94a8 | 566 | { |
| mdavis30 | 4:3c22d85a94a8 | 567 | PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled |
| mdavis30 | 4:3c22d85a94a8 | 568 | PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor |
| mdavis30 | 4:3c22d85a94a8 | 569 | wait(1); |
| mdavis30 | 4:3c22d85a94a8 | 570 | PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor |
| mdavis30 | 4:3c22d85a94a8 | 571 | wait(1); |
| mdavis30 | 4:3c22d85a94a8 | 572 | PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n"); |
| mdavis30 | 4:3c22d85a94a8 | 573 | count_while++; |
| mdavis30 | 4:3c22d85a94a8 | 574 | } |
| mdavis30 | 4:3c22d85a94a8 | 575 | } |
| mdavis30 | 4:3c22d85a94a8 | 576 | |
| mdavis30 | 3:1257a7d2eb3a | 577 | /*if(BLA_object.MC_readable()) //if you can read the motor controller do this... |
| mdavis30 | 0:381a84fad08b | 578 | { |
| mdavis30 | 0:381a84fad08b | 579 | //PC.printf("BATTERY LINEAR ACTUATOR"); |
| mdavis30 | 0:381a84fad08b | 580 | |
| mdavis30 | 0:381a84fad08b | 581 | |
| mdavis30 | 0:381a84fad08b | 582 | //PC.printf("Motor Controller response:\n"); |
| mdavis30 | 0:381a84fad08b | 583 | // while(MC.readable()) |
| mdavis30 | 0:381a84fad08b | 584 | // { |
| mdavis30 | 0:381a84fad08b | 585 | // PC.putc(MC.getc()); //this is a pass-through of the MC (getc) to the PC (putc) |
| mdavis30 | 0:381a84fad08b | 586 | // wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing) |
| mdavis30 | 0:381a84fad08b | 587 | // } |
| mdavis30 | 4:3c22d85a94a8 | 588 | }*/ |
| mdavis30 | 0:381a84fad08b | 589 | if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0)) |
| mdavis30 | 0:381a84fad08b | 590 | { |
| mdavis30 | 0:381a84fad08b | 591 | count ++; |
| mdavis30 | 0:381a84fad08b | 592 | //pc().printf("We have a small issue\n"); |
| mdavis30 | 0:381a84fad08b | 593 | if(count==10) |
| mdavis30 | 0:381a84fad08b | 594 | { |
| mdavis30 | 0:381a84fad08b | 595 | pc().printf("Bad pot issue\n"); |
| mdavis30 | 4:3c22d85a94a8 | 596 | //hBridge().stop(); |
| mdavis30 | 0:381a84fad08b | 597 | } |
| mdavis30 | 0:381a84fad08b | 598 | |
| mdavis30 | 0:381a84fad08b | 599 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 600 | else if ((5.0*ain.read())<1.0) |
| mdavis30 | 2:c3cb3ea3c9fa | 601 | { |
| mdavis30 | 2:c3cb3ea3c9fa | 602 | pc().printf("Hit the limit switch??\n"); |
| mdavis30 | 2:c3cb3ea3c9fa | 603 | hBridge().stop(); |
| mdavis30 | 2:c3cb3ea3c9fa | 604 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 605 | |
| mdavis30 | 0:381a84fad08b | 606 | //string snaps |
| mdavis30 | 0:381a84fad08b | 607 | else if (pvf().getVelocity() > 100) |
| mdavis30 | 0:381a84fad08b | 608 | { |
| mdavis30 | 0:381a84fad08b | 609 | //hBridge().stop(); |
| mdavis30 | 0:381a84fad08b | 610 | pc().printf("********** String broke? *********\n"); |
| mdavis30 | 4:3c22d85a94a8 | 611 | } |
| mdavis30 | 0:381a84fad08b | 612 | |
| mdavis30 | 0:381a84fad08b | 613 | } |
| mdavis30 | 0:381a84fad08b | 614 | } |