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@3:1257a7d2eb3a, 2017-06-30 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jun 30 18:39:25 2017 +0000
- Revision:
- 3:1257a7d2eb3a
- Parent:
- 2:c3cb3ea3c9fa
- Child:
- 4:3c22d85a94a8
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 | 0:381a84fad08b | 42 | /* ************************************************************************* */ |
| mdavis30 | 0:381a84fad08b | 43 | |
| mdavis30 | 3:1257a7d2eb3a | 44 | /* PID LOOP STUFF */ |
| mdavis30 | 3:1257a7d2eb3a | 45 | float la_setPoint = 0.00; //the IMU pitch angle we want (setpoint) |
| mdavis30 | 3:1257a7d2eb3a | 46 | |
| mdavis30 | 3:1257a7d2eb3a | 47 | float la_P_gain = 1.0; |
| mdavis30 | 3:1257a7d2eb3a | 48 | float la_I_gain = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 49 | float la_D_gain = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 50 | /* PID LOOP STUFF */ |
| mdavis30 | 3:1257a7d2eb3a | 51 | |
| mdavis30 | 3:1257a7d2eb3a | 52 | float IMU_pitch_angle = 0.00; |
| mdavis30 | 3:1257a7d2eb3a | 53 | |
| mdavis30 | 3:1257a7d2eb3a | 54 | bool motor_retracting = false; |
| mdavis30 | 3:1257a7d2eb3a | 55 | bool motor_extending = false; |
| mdavis30 | 3:1257a7d2eb3a | 56 | |
| mdavis30 | 0:381a84fad08b | 57 | void IMU_ticking() |
| mdavis30 | 0:381a84fad08b | 58 | { |
| mdavis30 | 0:381a84fad08b | 59 | led1 = !led1; //flash the IMU LED |
| mdavis30 | 3:1257a7d2eb3a | 60 | |
| mdavis30 | 3:1257a7d2eb3a | 61 | PC.printf("%s\n", IMU_STRING.c_str()); //if there's something there, print it |
| mdavis30 | 0:381a84fad08b | 62 | } |
| mdavis30 | 0:381a84fad08b | 63 | |
| mdavis30 | 0:381a84fad08b | 64 | void PRESSURE_ticking() |
| mdavis30 | 0:381a84fad08b | 65 | { |
| mdavis30 | 3:1257a7d2eb3a | 66 | PC.printf("pressure: %f mm \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845); //read the analog pin |
| mdavis30 | 3:1257a7d2eb3a | 67 | //this voltage has been checked and scaled properly (6/28/2017) |
| mdavis30 | 0:381a84fad08b | 68 | } |
| mdavis30 | 0:381a84fad08b | 69 | |
| mdavis30 | 0:381a84fad08b | 70 | void BCE_ticking() //new 6/5/17 |
| mdavis30 | 0:381a84fad08b | 71 | { |
| mdavis30 | 0:381a84fad08b | 72 | 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 | 73 | } |
| mdavis30 | 0:381a84fad08b | 74 | |
| mdavis30 | 0:381a84fad08b | 75 | int main() |
| mdavis30 | 0:381a84fad08b | 76 | { |
| mdavis30 | 3:1257a7d2eb3a | 77 | PC.baud(9600); //mbed to PC serial connection speed |
| mdavis30 | 0:381a84fad08b | 78 | //PC.baud(230400); |
| mdavis30 | 0:381a84fad08b | 79 | //got screwy when i changed it |
| mdavis30 | 0:381a84fad08b | 80 | hBridge().stop(); |
| mdavis30 | 3:1257a7d2eb3a | 81 | |
| mdavis30 | 3:1257a7d2eb3a | 82 | PC.printf("Linear Actuator PID Program Started 6/22/17\n"); |
| mdavis30 | 3:1257a7d2eb3a | 83 | |
| mdavis30 | 3:1257a7d2eb3a | 84 | systemTime().start(); //start the timer, needed for PID loop |
| mdavis30 | 3:1257a7d2eb3a | 85 | |
| mdavis30 | 3:1257a7d2eb3a | 86 | /* **************** Linear Actuator MOTOR CONTROLLER **************** */ |
| mdavis30 | 3:1257a7d2eb3a | 87 | Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class |
| mdavis30 | 3:1257a7d2eb3a | 88 | |
| mdavis30 | 3:1257a7d2eb3a | 89 | PC.printf("%s\n", BLA_object.Keyboard_U().c_str()); //velocity = 0, motor disabled |
| mdavis30 | 3:1257a7d2eb3a | 90 | |
| mdavis30 | 3:1257a7d2eb3a | 91 | PC.printf("%s\n", BLA_object.Keyboard_Q().c_str()); //turn off motor |
| mdavis30 | 3:1257a7d2eb3a | 92 | wait(1); |
| mdavis30 | 3:1257a7d2eb3a | 93 | PC.printf("%s\n", BLA_object.Keyboard_E().c_str()); //turn on motor |
| mdavis30 | 3:1257a7d2eb3a | 94 | wait(1); |
| mdavis30 | 3:1257a7d2eb3a | 95 | |
| mdavis30 | 0:381a84fad08b | 96 | //setup and start the adc. This runs on a fixed interval and is interrupt driven |
| mdavis30 | 0:381a84fad08b | 97 | adc().initialize(); |
| mdavis30 | 0:381a84fad08b | 98 | adc().start(); |
| mdavis30 | 0:381a84fad08b | 99 | |
| mdavis30 | 0:381a84fad08b | 100 | //Initialize the position velocity filter. This will consume a couple of seconds for |
| mdavis30 | 0:381a84fad08b | 101 | //the filter to converge |
| mdavis30 | 0:381a84fad08b | 102 | pvf().init(); |
| mdavis30 | 0:381a84fad08b | 103 | |
| mdavis30 | 0:381a84fad08b | 104 | ////CHANGED TO GLOBAL VARIABLES |
| mdavis30 | 0:381a84fad08b | 105 | float motor_cmd = 0.0; |
| mdavis30 | 0:381a84fad08b | 106 | // float positionCmd = 250.0; |
| mdavis30 | 0:381a84fad08b | 107 | float P = 0.10; |
| mdavis30 | 0:381a84fad08b | 108 | float I = 0.00; |
| mdavis30 | 0:381a84fad08b | 109 | float D = 0.00; |
| mdavis30 | 0:381a84fad08b | 110 | float count = 0.0; |
| mdavis30 | 0:381a84fad08b | 111 | //char userInput; //from Trent's code? |
| mdavis30 | 0:381a84fad08b | 112 | |
| mdavis30 | 3:1257a7d2eb3a | 113 | int la_step = 1; |
| mdavis30 | 3:1257a7d2eb3a | 114 | int la_setPoint_temp = 0; |
| mdavis30 | 3:1257a7d2eb3a | 115 | |
| mdavis30 | 0:381a84fad08b | 116 | hBridge().run(motor_cmd); |
| mdavis30 | 0:381a84fad08b | 117 | |
| mdavis30 | 0:381a84fad08b | 118 | //set the intial gains for the position controller |
| mdavis30 | 0:381a84fad08b | 119 | posCon().setPgain(P); |
| mdavis30 | 0:381a84fad08b | 120 | posCon().setIgain(I); |
| mdavis30 | 0:381a84fad08b | 121 | posCon().setDgain(D); |
| mdavis30 | 0:381a84fad08b | 122 | //posCon().writeSetPoint(positionCmd); |
| mdavis30 | 0:381a84fad08b | 123 | |
| mdavis30 | 0:381a84fad08b | 124 | /* *************************** LED *************************** */ |
| mdavis30 | 0:381a84fad08b | 125 | led1 = 1; //initial values |
| mdavis30 | 0:381a84fad08b | 126 | led2 = 1; |
| mdavis30 | 0:381a84fad08b | 127 | led3 = 1; |
| mdavis30 | 0:381a84fad08b | 128 | led4 = 1; |
| mdavis30 | 0:381a84fad08b | 129 | /* *************************** LED *************************** */ |
| mdavis30 | 0:381a84fad08b | 130 | |
| mdavis30 | 0:381a84fad08b | 131 | PC.printf("Program Started 6/5/17\n"); |
| mdavis30 | 0:381a84fad08b | 132 | //PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n"); |
| mdavis30 | 0:381a84fad08b | 133 | |
| mdavis30 | 0:381a84fad08b | 134 | /* *************************** Potentiometer *************************** */ |
| mdavis30 | 3:1257a7d2eb3a | 135 | //PRESSURE_ticker.attach(&PRESSURE_ticking, 1.0); |
| mdavis30 | 0:381a84fad08b | 136 | /* *************************** Potentiometer *************************** */ |
| mdavis30 | 0:381a84fad08b | 137 | |
| mdavis30 | 0:381a84fad08b | 138 | /* *************************** MOTOR CONTROLLER *************************** */ |
| mdavis30 | 3:1257a7d2eb3a | 139 | //Battery_Linear_Actuator BLA_object; //create the IMU object from the imported class |
| mdavis30 | 0:381a84fad08b | 140 | /* *************************** MOTOR CONTROLLER *************************** */ |
| mdavis30 | 0:381a84fad08b | 141 | |
| mdavis30 | 0:381a84fad08b | 142 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 143 | IMU_code IMU_object; //create the IMU object from the imported class |
| mdavis30 | 3:1257a7d2eb3a | 144 | //IMU_ticker.attach(&IMU_ticking, 5.0); |
| mdavis30 | 0:381a84fad08b | 145 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 146 | |
| mdavis30 | 0:381a84fad08b | 147 | /* *************************** BCE *************************** */ |
| mdavis30 | 3:1257a7d2eb3a | 148 | //float previous_positionCmd = -1; |
| mdavis30 | 3:1257a7d2eb3a | 149 | //BCE_ticker.attach(&BCE_ticking, 5.0); |
| mdavis30 | 0:381a84fad08b | 150 | /* *************************** BCE *************************** */ |
| mdavis30 | 0:381a84fad08b | 151 | |
| mdavis30 | 0:381a84fad08b | 152 | while(1) |
| mdavis30 | 0:381a84fad08b | 153 | { |
| mdavis30 | 0:381a84fad08b | 154 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 155 | IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop |
| mdavis30 | 3:1257a7d2eb3a | 156 | IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly? |
| mdavis30 | 3:1257a7d2eb3a | 157 | //PC.printf("pitch angle... %f set pitch angle: %f\n", IMU_pitch_angle, la_setPoint); |
| mdavis30 | 0:381a84fad08b | 158 | /* *************************** IMU *************************** */ |
| mdavis30 | 0:381a84fad08b | 159 | |
| mdavis30 | 0:381a84fad08b | 160 | /* Buoyancy Engine */ |
| mdavis30 | 0:381a84fad08b | 161 | // update the position velocity filter |
| mdavis30 | 0:381a84fad08b | 162 | pvf().update(); |
| mdavis30 | 0:381a84fad08b | 163 | |
| mdavis30 | 0:381a84fad08b | 164 | //update the controller with the current numbers in the position guesser |
| mdavis30 | 0:381a84fad08b | 165 | posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ; |
| mdavis30 | 0:381a84fad08b | 166 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 0:381a84fad08b | 167 | |
| mdavis30 | 0:381a84fad08b | 168 | /* Buoyancy Engine */ |
| mdavis30 | 0:381a84fad08b | 169 | |
| mdavis30 | 0:381a84fad08b | 170 | //FOR DEBUGGING |
| mdavis30 | 0:381a84fad08b | 171 | //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 | 172 | |
| mdavis30 | 0:381a84fad08b | 173 | //PC.printf("WHILE LOOP\n"); //DEBUG |
| mdavis30 | 0:381a84fad08b | 174 | if (PC.readable()) |
| mdavis30 | 0:381a84fad08b | 175 | { |
| mdavis30 | 3:1257a7d2eb3a | 176 | //led4 != led4; |
| mdavis30 | 3:1257a7d2eb3a | 177 | //PC.printf("DEBUG: PC IS READABLE\n"); //DEBUG |
| mdavis30 | 0:381a84fad08b | 178 | |
| mdavis30 | 3:1257a7d2eb3a | 179 | Key=PC.getc(); |
| mdavis30 | 3:1257a7d2eb3a | 180 | if(Key=='w' or Key =='W') |
| mdavis30 | 0:381a84fad08b | 181 | { |
| mdavis30 | 0:381a84fad08b | 182 | PC.printf("\nW received!\n"); |
| mdavis30 | 0:381a84fad08b | 183 | |
| mdavis30 | 0:381a84fad08b | 184 | posCon().writeSetPoint(positionCmd); |
| mdavis30 | 0:381a84fad08b | 185 | //posCon().setPgain(P); |
| mdavis30 | 0:381a84fad08b | 186 | //posCon().setIgain(I); |
| mdavis30 | 0:381a84fad08b | 187 | //posCon().setDgain(D); |
| mdavis30 | 0:381a84fad08b | 188 | hBridge().run(posCon().getOutput()); |
| mdavis30 | 0:381a84fad08b | 189 | |
| mdavis30 | 0:381a84fad08b | 190 | hBridge().reset(); |
| mdavis30 | 0:381a84fad08b | 191 | |
| mdavis30 | 0:381a84fad08b | 192 | count = 0; |
| mdavis30 | 0:381a84fad08b | 193 | |
| mdavis30 | 0:381a84fad08b | 194 | } |
| mdavis30 | 3:1257a7d2eb3a | 195 | |
| mdavis30 | 3:1257a7d2eb3a | 196 | /*else if (Key == 'k' or Key=='K') |
| mdavis30 | 3:1257a7d2eb3a | 197 | PC.printf("%s\n", BLA_object.Keyboard_K().c_str()); |
| mdavis30 | 3:1257a7d2eb3a | 198 | else if (Key == 'l' or Key == 'L') |
| mdavis30 | 3:1257a7d2eb3a | 199 | PC.printf("%s\n", BLA_object.Keyboard_L().c_str());*/ |
| mdavis30 | 0:381a84fad08b | 200 | |
| mdavis30 | 3:1257a7d2eb3a | 201 | /*else if(Key =='Q' or Key == 'q') //motor disabled |
| mdavis30 | 0:381a84fad08b | 202 | { |
| mdavis30 | 0:381a84fad08b | 203 | PC.printf("%s\n", BLA_object.Keyboard_Q()); |
| mdavis30 | 0:381a84fad08b | 204 | } |
| mdavis30 | 0:381a84fad08b | 205 | |
| mdavis30 | 3:1257a7d2eb3a | 206 | else if(Key =='E' or Key == 'e') //motor enabled |
| mdavis30 | 0:381a84fad08b | 207 | { |
| mdavis30 | 0:381a84fad08b | 208 | PC.printf("%s\n", BLA_object.Keyboard_E()); |
| mdavis30 | 3:1257a7d2eb3a | 209 | }*/ |
| mdavis30 | 0:381a84fad08b | 210 | |
| mdavis30 | 3:1257a7d2eb3a | 211 | else if(Key=='p' or Key == 'P') |
| mdavis30 | 3:1257a7d2eb3a | 212 | { |
| mdavis30 | 3:1257a7d2eb3a | 213 | PC.printf("### position is %s ###\n", BLA_object.get_pos().c_str()); //flip this back and forth |
| mdavis30 | 3:1257a7d2eb3a | 214 | wait(10); //for debugging |
| mdavis30 | 3:1257a7d2eb3a | 215 | // "-999999" means it is not working |
| mdavis30 | 3:1257a7d2eb3a | 216 | } |
| mdavis30 | 3:1257a7d2eb3a | 217 | else if (Key == 'A' or Key == 'a') |
| mdavis30 | 0:381a84fad08b | 218 | { |
| mdavis30 | 3:1257a7d2eb3a | 219 | PC.printf("A recieved\n"); |
| mdavis30 | 3:1257a7d2eb3a | 220 | la_setPoint=la_setPoint_temp; |
| mdavis30 | 3:1257a7d2eb3a | 221 | PC.printf("LA angle now set to %f\n", la_setPoint); |
| mdavis30 | 3:1257a7d2eb3a | 222 | } |
| mdavis30 | 3:1257a7d2eb3a | 223 | else if (Key=='-' or Key == '_') |
| mdavis30 | 0:381a84fad08b | 224 | { |
| mdavis30 | 3:1257a7d2eb3a | 225 | la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0; |
| mdavis30 | 3:1257a7d2eb3a | 226 | PC.printf("- recieved\n"); |
| mdavis30 | 3:1257a7d2eb3a | 227 | PC.printf("LA angle changed to: %d la_step: %d\n", la_setPoint_temp, la_step); |
| mdavis30 | 3:1257a7d2eb3a | 228 | } |
| mdavis30 | 3:1257a7d2eb3a | 229 | else if (Key =='=' or Key == '+') |
| mdavis30 | 3:1257a7d2eb3a | 230 | { |
| mdavis30 | 3:1257a7d2eb3a | 231 | la_setPoint_temp += la_step; //IMU_pitch_angle += 1.0; |
| mdavis30 | 3:1257a7d2eb3a | 232 | PC.printf("+ recieved\n"); |
| mdavis30 | 3:1257a7d2eb3a | 233 | PC.printf("LA angle changed to: %d\n", la_setPoint_temp); |
| mdavis30 | 0:381a84fad08b | 234 | } |
| mdavis30 | 3:1257a7d2eb3a | 235 | else if (Key == '0' or Key == ')') |
| mdavis30 | 0:381a84fad08b | 236 | { |
| mdavis30 | 3:1257a7d2eb3a | 237 | PC.printf(") recieved\n"); |
| mdavis30 | 3:1257a7d2eb3a | 238 | if (la_step == 1) |
| mdavis30 | 3:1257a7d2eb3a | 239 | { |
| mdavis30 | 3:1257a7d2eb3a | 240 | la_step = 5; |
| mdavis30 | 3:1257a7d2eb3a | 241 | } |
| mdavis30 | 3:1257a7d2eb3a | 242 | else if (la_step == 5) |
| mdavis30 | 3:1257a7d2eb3a | 243 | { |
| mdavis30 | 3:1257a7d2eb3a | 244 | la_step = 10; |
| mdavis30 | 3:1257a7d2eb3a | 245 | } |
| mdavis30 | 3:1257a7d2eb3a | 246 | else if (la_step == 10) |
| mdavis30 | 3:1257a7d2eb3a | 247 | { |
| mdavis30 | 3:1257a7d2eb3a | 248 | la_step = 15; |
| mdavis30 | 3:1257a7d2eb3a | 249 | } |
| mdavis30 | 3:1257a7d2eb3a | 250 | else if (la_step == 15) |
| mdavis30 | 3:1257a7d2eb3a | 251 | { |
| mdavis30 | 3:1257a7d2eb3a | 252 | la_step = 1; |
| mdavis30 | 3:1257a7d2eb3a | 253 | } |
| mdavis30 | 3:1257a7d2eb3a | 254 | PC.printf("LA Step Size Now %d\n", la_step); |
| mdavis30 | 0:381a84fad08b | 255 | } |
| mdavis30 | 3:1257a7d2eb3a | 256 | else if (Key=='[' or Key == '{') |
| mdavis30 | 3:1257a7d2eb3a | 257 | { |
| mdavis30 | 3:1257a7d2eb3a | 258 | la_P_gain -= 0.1; |
| mdavis30 | 3:1257a7d2eb3a | 259 | PC.printf("[ key pressed\n"); |
| mdavis30 | 3:1257a7d2eb3a | 260 | PC.printf("P gain is now %f\n", la_P_gain); |
| mdavis30 | 3:1257a7d2eb3a | 261 | |
| mdavis30 | 3:1257a7d2eb3a | 262 | } |
| mdavis30 | 3:1257a7d2eb3a | 263 | else if (Key==']' or Key == '}') |
| mdavis30 | 3:1257a7d2eb3a | 264 | { |
| mdavis30 | 3:1257a7d2eb3a | 265 | la_P_gain += 0.1; |
| mdavis30 | 3:1257a7d2eb3a | 266 | PC.printf("] key pressed\n"); |
| mdavis30 | 3:1257a7d2eb3a | 267 | PC.printf("P gain is now %f\n", la_P_gain); |
| mdavis30 | 3:1257a7d2eb3a | 268 | |
| mdavis30 | 3:1257a7d2eb3a | 269 | } |
| mdavis30 | 3:1257a7d2eb3a | 270 | else if (Key==';') |
| mdavis30 | 3:1257a7d2eb3a | 271 | { |
| mdavis30 | 3:1257a7d2eb3a | 272 | la_I_gain -= 0.1; |
| mdavis30 | 3:1257a7d2eb3a | 273 | PC.printf("[ key pressed\n"); |
| mdavis30 | 3:1257a7d2eb3a | 274 | PC.printf("P gain is now %f\n", la_P_gain); |
| mdavis30 | 3:1257a7d2eb3a | 275 | |
| mdavis30 | 0:381a84fad08b | 276 | |
| mdavis30 | 0:381a84fad08b | 277 | } |
| mdavis30 | 3:1257a7d2eb3a | 278 | else if (Key=='\'') |
| mdavis30 | 3:1257a7d2eb3a | 279 | la_I_gain += 0.1; |
| mdavis30 | 3:1257a7d2eb3a | 280 | else if (Key=='.') |
| mdavis30 | 3:1257a7d2eb3a | 281 | la_D_gain -= 0.1; |
| mdavis30 | 3:1257a7d2eb3a | 282 | else if (Key=='/') |
| mdavis30 | 3:1257a7d2eb3a | 283 | la_D_gain += 0.1; |
| mdavis30 | 3:1257a7d2eb3a | 284 | |
| mdavis30 | 0:381a84fad08b | 285 | else if(Key=='!') //RESET THE MBED |
| mdavis30 | 0:381a84fad08b | 286 | { |
| mdavis30 | 0:381a84fad08b | 287 | PC.printf("MBED RESET KEY (!) PRESSED\n"); |
| mdavis30 | 0:381a84fad08b | 288 | PC.printf("Linear Actuator Motor disabled! Please re-enable.\n"); |
| mdavis30 | 0:381a84fad08b | 289 | //disable the motor |
| mdavis30 | 0:381a84fad08b | 290 | BLA_object.Keyboard_Q(); //DISABLE THE MOTOR |
| mdavis30 | 0:381a84fad08b | 291 | wait(0.5); //500 milliseconds |
| mdavis30 | 0:381a84fad08b | 292 | mbed_reset(); //reset the mbed! |
| mdavis30 | 0:381a84fad08b | 293 | } |
| mdavis30 | 0:381a84fad08b | 294 | |
| mdavis30 | 0:381a84fad08b | 295 | else if(Key =='H') //homing sequence |
| mdavis30 | 0:381a84fad08b | 296 | { |
| mdavis30 | 3:1257a7d2eb3a | 297 | PC.printf("### homing the device ###"); |
| mdavis30 | 3:1257a7d2eb3a | 298 | BLA_object.Keyboard_H(); |
| mdavis30 | 3:1257a7d2eb3a | 299 | wait(10); //for debugging |
| mdavis30 | 3:1257a7d2eb3a | 300 | |
| mdavis30 | 3:1257a7d2eb3a | 301 | PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str()); //flip this back and forth |
| mdavis30 | 3:1257a7d2eb3a | 302 | wait(10); //for debugging |
| mdavis30 | 0:381a84fad08b | 303 | } |
| mdavis30 | 0:381a84fad08b | 304 | //check command against desired control buttons |
| mdavis30 | 0:381a84fad08b | 305 | else if (Key == 'z' or Key =='Z') { |
| mdavis30 | 0:381a84fad08b | 306 | //increment the duty cycle |
| mdavis30 | 0:381a84fad08b | 307 | positionCmd += 10.0 ; |
| mdavis30 | 0:381a84fad08b | 308 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 0:381a84fad08b | 309 | } |
| mdavis30 | 0:381a84fad08b | 310 | else if (Key == 'x' or Key == 'X') { |
| mdavis30 | 0:381a84fad08b | 311 | //decrement the duty cycle |
| mdavis30 | 0:381a84fad08b | 312 | positionCmd -= 10.0 ; |
| mdavis30 | 0:381a84fad08b | 313 | PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB |
| mdavis30 | 0:381a84fad08b | 314 | } |
| mdavis30 | 0:381a84fad08b | 315 | |
| mdavis30 | 0:381a84fad08b | 316 | else |
| mdavis30 | 0:381a84fad08b | 317 | { |
| mdavis30 | 0:381a84fad08b | 318 | PC.printf("\n%c received!\n", Key); |
| mdavis30 | 0:381a84fad08b | 319 | PC.printf("\nDoing nothing.\n"); |
| mdavis30 | 0:381a84fad08b | 320 | } |
| mdavis30 | 0:381a84fad08b | 321 | |
| mdavis30 | 0:381a84fad08b | 322 | wait_us(100); //for PC readable |
| mdavis30 | 3:1257a7d2eb3a | 323 | //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 | 324 | //BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| mdavis30 | 3:1257a7d2eb3a | 325 | } |
| mdavis30 | 0:381a84fad08b | 326 | |
| mdavis30 | 3:1257a7d2eb3a | 327 | //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 | 328 | BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str(); |
| mdavis30 | 0:381a84fad08b | 329 | wait_us(100); //for PC readable (0.1 ms) |
| mdavis30 | 0:381a84fad08b | 330 | |
| mdavis30 | 3:1257a7d2eb3a | 331 | /*if(BLA_object.MC_readable()) //if you can read the motor controller do this... |
| mdavis30 | 0:381a84fad08b | 332 | { |
| mdavis30 | 0:381a84fad08b | 333 | //PC.printf("BATTERY LINEAR ACTUATOR"); |
| mdavis30 | 0:381a84fad08b | 334 | |
| mdavis30 | 0:381a84fad08b | 335 | |
| mdavis30 | 0:381a84fad08b | 336 | //PC.printf("Motor Controller response:\n"); |
| mdavis30 | 0:381a84fad08b | 337 | // while(MC.readable()) |
| mdavis30 | 0:381a84fad08b | 338 | // { |
| mdavis30 | 0:381a84fad08b | 339 | // PC.putc(MC.getc()); //this is a pass-through of the MC (getc) to the PC (putc) |
| mdavis30 | 0:381a84fad08b | 340 | // wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing) |
| mdavis30 | 0:381a84fad08b | 341 | // } |
| mdavis30 | 0:381a84fad08b | 342 | } |
| mdavis30 | 0:381a84fad08b | 343 | if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0)) |
| mdavis30 | 0:381a84fad08b | 344 | { |
| mdavis30 | 0:381a84fad08b | 345 | count ++; |
| mdavis30 | 0:381a84fad08b | 346 | //pc().printf("We have a small issue\n"); |
| mdavis30 | 0:381a84fad08b | 347 | if(count==10) |
| mdavis30 | 0:381a84fad08b | 348 | { |
| mdavis30 | 0:381a84fad08b | 349 | pc().printf("Bad pot issue\n"); |
| mdavis30 | 0:381a84fad08b | 350 | hBridge().stop(); |
| mdavis30 | 0:381a84fad08b | 351 | } |
| mdavis30 | 0:381a84fad08b | 352 | |
| mdavis30 | 0:381a84fad08b | 353 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 354 | else if ((5.0*ain.read())<1.0) |
| mdavis30 | 2:c3cb3ea3c9fa | 355 | { |
| mdavis30 | 2:c3cb3ea3c9fa | 356 | pc().printf("Hit the limit switch??\n"); |
| mdavis30 | 2:c3cb3ea3c9fa | 357 | hBridge().stop(); |
| mdavis30 | 2:c3cb3ea3c9fa | 358 | } |
| mdavis30 | 2:c3cb3ea3c9fa | 359 | |
| mdavis30 | 0:381a84fad08b | 360 | //string snaps |
| mdavis30 | 0:381a84fad08b | 361 | else if (pvf().getVelocity() > 100) |
| mdavis30 | 0:381a84fad08b | 362 | { |
| mdavis30 | 0:381a84fad08b | 363 | //hBridge().stop(); |
| mdavis30 | 0:381a84fad08b | 364 | pc().printf("********** String broke? *********\n"); |
| mdavis30 | 3:1257a7d2eb3a | 365 | }*/ |
| mdavis30 | 0:381a84fad08b | 366 | |
| mdavis30 | 0:381a84fad08b | 367 | } |
| mdavis30 | 0:381a84fad08b | 368 | } |