FSG
/
Battery_Linear_Actuator
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.
Dependents: 7_21_17_FSG 7_26_17_FSG
Fork of
Battery_Linear_Actuator_
by 
FSG
Battery_Linear_Actuator.cpp@1:ca2454ef80d9, 2017-06-30 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jun 30 18:32:40 2017 +0000
- Revision:
- 1:ca2454ef80d9
- Parent:
- 0:645ad86abcba
- Child:
- 2:627fd46c2b99
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| mdavis30 | 0:645ad86abcba | 1 | #include "Battery_Linear_Actuator.h" |
| mdavis30 | 0:645ad86abcba | 2 | |
| mdavis30 | 1:ca2454ef80d9 | 3 | #include "StaticDefs.hpp" |
| mdavis30 | 1:ca2454ef80d9 | 4 | |
| mdavis30 | 0:645ad86abcba | 5 | #include "MODSERIAL.h" |
| mdavis30 | 0:645ad86abcba | 6 | Serial MC(p28,p27); //tx, rx (pins verified) |
| mdavis30 | 0:645ad86abcba | 7 | |
| mdavis30 | 0:645ad86abcba | 8 | //CONSTRUCTOR |
| mdavis30 | 0:645ad86abcba | 9 | Battery_Linear_Actuator::Battery_Linear_Actuator() |
| mdavis30 | 0:645ad86abcba | 10 | { |
| mdavis30 | 0:645ad86abcba | 11 | MC.baud(9600); //motor controller to MBED connection speed |
| mdavis30 | 1:ca2454ef80d9 | 12 | |
| mdavis30 | 1:ca2454ef80d9 | 13 | MC.printf("sp100"); //set the speed to 100, test this until I can safely set to 500 |
| mdavis30 | 1:ca2454ef80d9 | 14 | |
| mdavis30 | 0:645ad86abcba | 15 | motor_absolute_position = 0; //initialize variables here |
| mdavis30 | 0:645ad86abcba | 16 | |
| mdavis30 | 0:645ad86abcba | 17 | linear_actuator_step_size = 1000; //default confirmed with Dan Edwards |
| mdavis30 | 0:645ad86abcba | 18 | linear_actuator_motor_speed = 500; //5 seconds of full travel between 0 and 175,000 (1000 is about 3 seconds) |
| mdavis30 | 0:645ad86abcba | 19 | |
| mdavis30 | 1:ca2454ef80d9 | 20 | map_check = 0; //remove? |
| mdavis30 | 1:ca2454ef80d9 | 21 | |
| mdavis30 | 1:ca2454ef80d9 | 22 | |
| mdavis30 | 1:ca2454ef80d9 | 23 | // la_P_gain = 1.0; //was 0.10 |
| mdavis30 | 1:ca2454ef80d9 | 24 | // la_I_gain = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 25 | // la_D_gain = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 26 | curr_time = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 27 | dt = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 28 | la_error = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 29 | la_error_sum = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 30 | la_derivative_error = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 31 | lastErr = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 32 | lastTime = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 33 | |
| mdavis30 | 1:ca2454ef80d9 | 34 | la_integral = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 35 | la_output = 0.00; |
| mdavis30 | 1:ca2454ef80d9 | 36 | |
| mdavis30 | 1:ca2454ef80d9 | 37 | actual_position = 0; |
| mdavis30 | 0:645ad86abcba | 38 | } |
| mdavis30 | 0:645ad86abcba | 39 | |
| mdavis30 | 0:645ad86abcba | 40 | //KEYBOARD FUNCTIONS |
| mdavis30 | 0:645ad86abcba | 41 | int Battery_Linear_Actuator::MC_readable() |
| mdavis30 | 0:645ad86abcba | 42 | { |
| mdavis30 | 0:645ad86abcba | 43 | return MC.readable(); |
| mdavis30 | 0:645ad86abcba | 44 | } |
| mdavis30 | 0:645ad86abcba | 45 | |
| mdavis30 | 1:ca2454ef80d9 | 46 | char Battery_Linear_Actuator::MC_getc() |
| mdavis30 | 1:ca2454ef80d9 | 47 | { |
| mdavis30 | 1:ca2454ef80d9 | 48 | return MC.getc(); //return this character to whatever is calling the function, useful for getting Motor Controller data |
| mdavis30 | 1:ca2454ef80d9 | 49 | } |
| mdavis30 | 1:ca2454ef80d9 | 50 | |
| mdavis30 | 1:ca2454ef80d9 | 51 | string Battery_Linear_Actuator::MC_readable_redux() |
| mdavis30 | 1:ca2454ef80d9 | 52 | { |
| mdavis30 | 1:ca2454ef80d9 | 53 | string bla_string = ""; |
| mdavis30 | 1:ca2454ef80d9 | 54 | |
| mdavis30 | 1:ca2454ef80d9 | 55 | //MC.printf("pos\r"); |
| mdavis30 | 1:ca2454ef80d9 | 56 | wait(0.1); //THIS SHOULD WORK! |
| mdavis30 | 1:ca2454ef80d9 | 57 | |
| mdavis30 | 1:ca2454ef80d9 | 58 | if(MC.readable()) //if you can read the motor controller do this... |
| mdavis30 | 1:ca2454ef80d9 | 59 | { |
| mdavis30 | 1:ca2454ef80d9 | 60 | //bla_string += "\nMC readable\n"; |
| mdavis30 | 1:ca2454ef80d9 | 61 | |
| mdavis30 | 1:ca2454ef80d9 | 62 | while(MC.readable()) |
| mdavis30 | 1:ca2454ef80d9 | 63 | { |
| mdavis30 | 1:ca2454ef80d9 | 64 | //strcat(bla_array, MC.getc()); //this is a pass-through of the MC (getc) to the PC (putc) |
| mdavis30 | 1:ca2454ef80d9 | 65 | |
| mdavis30 | 1:ca2454ef80d9 | 66 | bla_string += MC.getc(); |
| mdavis30 | 1:ca2454ef80d9 | 67 | wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing) |
| mdavis30 | 1:ca2454ef80d9 | 68 | } |
| mdavis30 | 1:ca2454ef80d9 | 69 | } |
| mdavis30 | 1:ca2454ef80d9 | 70 | |
| mdavis30 | 1:ca2454ef80d9 | 71 | return bla_string; //return the character array as a string |
| mdavis30 | 1:ca2454ef80d9 | 72 | } |
| mdavis30 | 1:ca2454ef80d9 | 73 | |
| mdavis30 | 0:645ad86abcba | 74 | string Battery_Linear_Actuator::Keyboard_O() |
| mdavis30 | 0:645ad86abcba | 75 | { |
| mdavis30 | 0:645ad86abcba | 76 | //PC.printf("MBED_m_a_p: %d \n", motor_absolute_position); //linear actuator position |
| mdavis30 | 0:645ad86abcba | 77 | |
| mdavis30 | 0:645ad86abcba | 78 | MC.printf("pos\r"); //get position from motor controller...maybe rewrite this |
| mdavis30 | 0:645ad86abcba | 79 | //get this from the Faulhaber Motor Controller in MATLAB? |
| mdavis30 | 0:645ad86abcba | 80 | |
| mdavis30 | 0:645ad86abcba | 81 | char char_buffer [60]; |
| mdavis30 | 0:645ad86abcba | 82 | string Battery_Position_String; |
| mdavis30 | 0:645ad86abcba | 83 | sprintf(char_buffer,"MBED_m_a_p: %d \n", motor_absolute_position); //linear actuator position |
| mdavis30 | 0:645ad86abcba | 84 | Battery_Position_String = char_buffer; |
| mdavis30 | 0:645ad86abcba | 85 | return Battery_Position_String; |
| mdavis30 | 0:645ad86abcba | 86 | } |
| mdavis30 | 0:645ad86abcba | 87 | |
| mdavis30 | 0:645ad86abcba | 88 | string Battery_Linear_Actuator::Keyboard_H() //Home linear actuator |
| mdavis30 | 0:645ad86abcba | 89 | { |
| mdavis30 | 0:645ad86abcba | 90 | //reset position in program: |
| mdavis30 | 0:645ad86abcba | 91 | motor_absolute_position = 0; |
| mdavis30 | 0:645ad86abcba | 92 | |
| mdavis30 | 0:645ad86abcba | 93 | MC.printf("hosp-100\r"); wait(0.1); //homing speed -100 (verified |
| mdavis30 | 0:645ad86abcba | 94 | MC.printf("ghosp\r"); wait(0.1);//check homing speed |
| mdavis30 | 0:645ad86abcba | 95 | MC.printf("SHL1\r"); wait(0.1);//setup fault and AnIn |
| mdavis30 | 0:645ad86abcba | 96 | MC.printf("gohoseq\r"); wait(0.1);//enable the sequence |
| mdavis30 | 0:645ad86abcba | 97 | |
| mdavis30 | 0:645ad86abcba | 98 | char homing_string[80]; |
| mdavis30 | 0:645ad86abcba | 99 | sprintf(homing_string,"\nH received! (Homing Sequence In Progress)\nMBED_m_a_p: %d \n", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 100 | return homing_string; |
| mdavis30 | 0:645ad86abcba | 101 | } |
| mdavis30 | 0:645ad86abcba | 102 | |
| mdavis30 | 1:ca2454ef80d9 | 103 | string Battery_Linear_Actuator::Keyboard_J() //L.A. POSITION ONLY |
| mdavis30 | 1:ca2454ef80d9 | 104 | { |
| mdavis30 | 1:ca2454ef80d9 | 105 | MC.printf("pos\r"); |
| mdavis30 | 1:ca2454ef80d9 | 106 | return "\nL.A. POSITION COMMAND RECEIVED.\n"; |
| mdavis30 | 1:ca2454ef80d9 | 107 | } |
| mdavis30 | 1:ca2454ef80d9 | 108 | |
| mdavis30 | 1:ca2454ef80d9 | 109 | string Battery_Linear_Actuator::Velocity(int velocity) //EXTEND with velocity command |
| mdavis30 | 1:ca2454ef80d9 | 110 | { |
| mdavis30 | 1:ca2454ef80d9 | 111 | string la_position = Battery_Linear_Actuator::get_pos(); //get the position data in string format |
| mdavis30 | 1:ca2454ef80d9 | 112 | |
| mdavis30 | 1:ca2454ef80d9 | 113 | //https://stackoverflow.com/questions/13294067/how-to-convert-string-to-char-array-in-c |
| mdavis30 | 1:ca2454ef80d9 | 114 | char la_position_char[1024]; |
| mdavis30 | 1:ca2454ef80d9 | 115 | strcpy(la_position_char, la_position.c_str()); //copy it to the array? |
| mdavis30 | 1:ca2454ef80d9 | 116 | |
| mdavis30 | 1:ca2454ef80d9 | 117 | //back to the drawing board |
| mdavis30 | 1:ca2454ef80d9 | 118 | //https://developer.mbed.org/questions/2910/Convert-string-to-int/ |
| mdavis30 | 1:ca2454ef80d9 | 119 | |
| mdavis30 | 1:ca2454ef80d9 | 120 | int int_position = atoi(la_position_char); |
| mdavis30 | 1:ca2454ef80d9 | 121 | |
| mdavis30 | 1:ca2454ef80d9 | 122 | if (int_position >= 0 && int_position <= 175000) |
| mdavis30 | 1:ca2454ef80d9 | 123 | { |
| mdavis30 | 1:ca2454ef80d9 | 124 | MC.printf("en\r"); |
| mdavis30 | 1:ca2454ef80d9 | 125 | MC.printf("V%d\r", velocity); |
| mdavis30 | 1:ca2454ef80d9 | 126 | |
| mdavis30 | 1:ca2454ef80d9 | 127 | char velocity_string[80]; |
| mdavis30 | 1:ca2454ef80d9 | 128 | sprintf(velocity_string, "\nVELOCITY COMMAND: V%d.\n", velocity); |
| mdavis30 | 1:ca2454ef80d9 | 129 | |
| mdavis30 | 1:ca2454ef80d9 | 130 | return velocity_string; |
| mdavis30 | 1:ca2454ef80d9 | 131 | } |
| mdavis30 | 1:ca2454ef80d9 | 132 | |
| mdavis30 | 1:ca2454ef80d9 | 133 | else |
| mdavis30 | 1:ca2454ef80d9 | 134 | return "outside of bounds"; |
| mdavis30 | 1:ca2454ef80d9 | 135 | } |
| mdavis30 | 1:ca2454ef80d9 | 136 | |
| mdavis30 | 1:ca2454ef80d9 | 137 | string Battery_Linear_Actuator::velocity_only(int velocity) //EXTEND with velocity command |
| mdavis30 | 1:ca2454ef80d9 | 138 | { |
| mdavis30 | 1:ca2454ef80d9 | 139 | //this uses the position record saved in the object |
| mdavis30 | 1:ca2454ef80d9 | 140 | if (actual_position >= 0 && actual_position <= 175000) |
| mdavis30 | 1:ca2454ef80d9 | 141 | { |
| mdavis30 | 1:ca2454ef80d9 | 142 | MC.printf("en\r"); |
| mdavis30 | 1:ca2454ef80d9 | 143 | MC.printf("V%d\r", velocity); |
| mdavis30 | 1:ca2454ef80d9 | 144 | |
| mdavis30 | 1:ca2454ef80d9 | 145 | char velocity_string[80]; |
| mdavis30 | 1:ca2454ef80d9 | 146 | sprintf(velocity_string, "\nVELOCITY COMMAND: V%d.\n", velocity); |
| mdavis30 | 1:ca2454ef80d9 | 147 | |
| mdavis30 | 1:ca2454ef80d9 | 148 | return velocity_string; |
| mdavis30 | 1:ca2454ef80d9 | 149 | } |
| mdavis30 | 1:ca2454ef80d9 | 150 | |
| mdavis30 | 1:ca2454ef80d9 | 151 | else |
| mdavis30 | 1:ca2454ef80d9 | 152 | return "outside of bounds"; //maybe change this |
| mdavis30 | 1:ca2454ef80d9 | 153 | } |
| mdavis30 | 1:ca2454ef80d9 | 154 | |
| mdavis30 | 1:ca2454ef80d9 | 155 | string Battery_Linear_Actuator::Keyboard_K() //EXTEND with velocity command |
| mdavis30 | 1:ca2454ef80d9 | 156 | { |
| mdavis30 | 1:ca2454ef80d9 | 157 | if (motor_absolute_position >= 0 && motor_absolute_position <= 175000) |
| mdavis30 | 1:ca2454ef80d9 | 158 | { |
| mdavis30 | 1:ca2454ef80d9 | 159 | MC.printf("en\r"); |
| mdavis30 | 1:ca2454ef80d9 | 160 | MC.printf("V50\r"); |
| mdavis30 | 1:ca2454ef80d9 | 161 | return "\nVELOCITY COMMAND. Extending.\n"; |
| mdavis30 | 1:ca2454ef80d9 | 162 | } |
| mdavis30 | 1:ca2454ef80d9 | 163 | else |
| mdavis30 | 1:ca2454ef80d9 | 164 | return "\nmotor position out of bounds (below 0 or above 175000)\n"; |
| mdavis30 | 1:ca2454ef80d9 | 165 | } |
| mdavis30 | 1:ca2454ef80d9 | 166 | |
| mdavis30 | 1:ca2454ef80d9 | 167 | string Battery_Linear_Actuator::Keyboard_L() //RETRACT with velocity command |
| mdavis30 | 1:ca2454ef80d9 | 168 | { |
| mdavis30 | 1:ca2454ef80d9 | 169 | if (motor_absolute_position >= 0 && motor_absolute_position <= 175000) |
| mdavis30 | 1:ca2454ef80d9 | 170 | { |
| mdavis30 | 1:ca2454ef80d9 | 171 | MC.printf("en\r"); |
| mdavis30 | 1:ca2454ef80d9 | 172 | MC.printf("V-50\r"); |
| mdavis30 | 1:ca2454ef80d9 | 173 | return "\nVELOCITY COMMAND. Retracting.\n"; |
| mdavis30 | 1:ca2454ef80d9 | 174 | } |
| mdavis30 | 1:ca2454ef80d9 | 175 | else |
| mdavis30 | 1:ca2454ef80d9 | 176 | return "\nmotor position out of bounds (below 0 or above 175000)\n"; |
| mdavis30 | 1:ca2454ef80d9 | 177 | } |
| mdavis30 | 1:ca2454ef80d9 | 178 | string Battery_Linear_Actuator::Keyboard_U() |
| mdavis30 | 1:ca2454ef80d9 | 179 | { |
| mdavis30 | 1:ca2454ef80d9 | 180 | MC.printf("v0\r"); //velocity 0 |
| mdavis30 | 1:ca2454ef80d9 | 181 | MC.printf("di\r"); //motor disabled |
| mdavis30 | 1:ca2454ef80d9 | 182 | return "\nVelocity=0, L.A. motor disabled.\n"; |
| mdavis30 | 1:ca2454ef80d9 | 183 | } |
| mdavis30 | 1:ca2454ef80d9 | 184 | |
| mdavis30 | 0:645ad86abcba | 185 | string Battery_Linear_Actuator::Keyboard_E() //enable linear actuator |
| mdavis30 | 0:645ad86abcba | 186 | { |
| mdavis30 | 0:645ad86abcba | 187 | MC.printf("en\r"); |
| mdavis30 | 0:645ad86abcba | 188 | return "\nE received!\nMOTOR ENABLED!"; |
| mdavis30 | 0:645ad86abcba | 189 | } |
| mdavis30 | 0:645ad86abcba | 190 | |
| mdavis30 | 0:645ad86abcba | 191 | string Battery_Linear_Actuator::Keyboard_Q() //disable linear actuator |
| mdavis30 | 0:645ad86abcba | 192 | { |
| mdavis30 | 0:645ad86abcba | 193 | MC.printf("di\r"); |
| mdavis30 | 0:645ad86abcba | 194 | return "\nQ received!\nMOTOR DISABLED!"; |
| mdavis30 | 0:645ad86abcba | 195 | } |
| mdavis30 | 0:645ad86abcba | 196 | |
| mdavis30 | 0:645ad86abcba | 197 | string Battery_Linear_Actuator::Keyboard_EQUAL_KEY() //INCREASE L.A. step size |
| mdavis30 | 0:645ad86abcba | 198 | { |
| mdavis30 | 0:645ad86abcba | 199 | if (linear_actuator_step_size <= 9800) //arbitrary stop at 10000 |
| mdavis30 | 0:645ad86abcba | 200 | linear_actuator_step_size += 200; |
| mdavis30 | 0:645ad86abcba | 201 | char bla_string[80]; |
| mdavis30 | 1:ca2454ef80d9 | 202 | sprintf(bla_string, "\n(MBED) Linear Actuator step size INCREASED\nLA_SZ: %d", linear_actuator_step_size); |
| mdavis30 | 0:645ad86abcba | 203 | |
| mdavis30 | 0:645ad86abcba | 204 | return bla_string; |
| mdavis30 | 0:645ad86abcba | 205 | } |
| mdavis30 | 0:645ad86abcba | 206 | |
| mdavis30 | 0:645ad86abcba | 207 | string Battery_Linear_Actuator::Keyboard_DASH_KEY() //DECREASE L.A. step size |
| mdavis30 | 0:645ad86abcba | 208 | { |
| mdavis30 | 0:645ad86abcba | 209 | if (linear_actuator_step_size >= 200) |
| mdavis30 | 0:645ad86abcba | 210 | linear_actuator_step_size -= 200; |
| mdavis30 | 0:645ad86abcba | 211 | char bla_string[80]; |
| mdavis30 | 1:ca2454ef80d9 | 212 | sprintf(bla_string, "\n(MBED) Linear Actuator step size DECREASED\nLA_SZ: %d", linear_actuator_step_size); |
| mdavis30 | 0:645ad86abcba | 213 | |
| mdavis30 | 0:645ad86abcba | 214 | return bla_string; |
| mdavis30 | 0:645ad86abcba | 215 | } |
| mdavis30 | 0:645ad86abcba | 216 | |
| mdavis30 | 0:645ad86abcba | 217 | string Battery_Linear_Actuator::Keyboard_LEFT_BRACKET() |
| mdavis30 | 0:645ad86abcba | 218 | { //100 to 30000, DECREASE (100 increments) |
| mdavis30 | 1:ca2454ef80d9 | 219 | if (linear_actuator_motor_speed >= 200) |
| mdavis30 | 1:ca2454ef80d9 | 220 | linear_actuator_motor_speed -= 200; |
| mdavis30 | 0:645ad86abcba | 221 | //PC.printf("\n(MBED) Linear Actuator Motor Speed INCREASED: sp%d\n", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 222 | MC.printf("sp%d\r", linear_actuator_motor_speed); //retesting linear actuator motor speed 5/11/17 |
| mdavis30 | 0:645ad86abcba | 223 | //PC.printf("\nLAMSP: %d\n", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 224 | |
| mdavis30 | 0:645ad86abcba | 225 | char str[80]; |
| mdavis30 | 0:645ad86abcba | 226 | sprintf(str,"(MBED) Linear Actuator Motor Speed DECREASED\nLA_SP: %d", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 227 | |
| mdavis30 | 0:645ad86abcba | 228 | return str; |
| mdavis30 | 0:645ad86abcba | 229 | } |
| mdavis30 | 0:645ad86abcba | 230 | |
| mdavis30 | 0:645ad86abcba | 231 | string Battery_Linear_Actuator::Keyboard_RIGHT_BRACKET() //disable linear actuator |
| mdavis30 | 0:645ad86abcba | 232 | { //100 to 30000, INCREASE (100 increments) |
| mdavis30 | 1:ca2454ef80d9 | 233 | if (linear_actuator_motor_speed <= 29800) |
| mdavis30 | 1:ca2454ef80d9 | 234 | linear_actuator_motor_speed += 200; |
| mdavis30 | 0:645ad86abcba | 235 | //PC.printf("\n(MBED) Linear Actuator Motor Speed INCREASED: sp%d\n", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 236 | MC.printf("sp%d\r", linear_actuator_motor_speed); //retesting linear actuator motor speed 5/11/17 |
| mdavis30 | 0:645ad86abcba | 237 | //PC.printf("\nLAMSP: %d\n", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 238 | |
| mdavis30 | 0:645ad86abcba | 239 | char str[80]; |
| mdavis30 | 0:645ad86abcba | 240 | sprintf(str,"(MBED) Linear Actuator Motor Speed INCREASED\nLA_SP: %d", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 241 | |
| mdavis30 | 0:645ad86abcba | 242 | return str; |
| mdavis30 | 0:645ad86abcba | 243 | } |
| mdavis30 | 0:645ad86abcba | 244 | |
| mdavis30 | 0:645ad86abcba | 245 | //http://stackoverflow.com/questions/8011700/how-do-i-extract-specific-n-bits-of-a-32-bit-unsigned-integer-in-c |
| mdavis30 | 0:645ad86abcba | 246 | unsigned createMask(unsigned a, unsigned b) { |
| mdavis30 | 0:645ad86abcba | 247 | unsigned r = 0; |
| mdavis30 | 0:645ad86abcba | 248 | for (unsigned i=a; i<=b; i++) |
| mdavis30 | 0:645ad86abcba | 249 | r |= 1 << i; |
| mdavis30 | 0:645ad86abcba | 250 | |
| mdavis30 | 0:645ad86abcba | 251 | return r; |
| mdavis30 | 0:645ad86abcba | 252 | } |
| mdavis30 | 0:645ad86abcba | 253 | |
| mdavis30 | 1:ca2454ef80d9 | 254 | //RETRACT |
| mdavis30 | 0:645ad86abcba | 255 | string Battery_Linear_Actuator::Keyboard_A() |
| mdavis30 | 0:645ad86abcba | 256 | { |
| mdavis30 | 1:ca2454ef80d9 | 257 | // float time_read = systemTime().read(); //added #include "StaticDefs.hpp" on 6/14/17 |
| mdavis30 | 1:ca2454ef80d9 | 258 | // char position[80]; |
| mdavis30 | 1:ca2454ef80d9 | 259 | // sprintf(position, "\nSYSTEM TIME IS %f\n", time_read); //SYSTEM TIME IS 24.918480 |
| mdavis30 | 1:ca2454ef80d9 | 260 | // return position; |
| mdavis30 | 1:ca2454ef80d9 | 261 | |
| mdavis30 | 0:645ad86abcba | 262 | if (motor_absolute_position >= 0) |
| mdavis30 | 0:645ad86abcba | 263 | { |
| mdavis30 | 0:645ad86abcba | 264 | //explicitly written, check if this will not go past 175000 |
| mdavis30 | 0:645ad86abcba | 265 | map_check = motor_absolute_position - linear_actuator_step_size; |
| mdavis30 | 0:645ad86abcba | 266 | if (map_check >= 0) |
| mdavis30 | 0:645ad86abcba | 267 | motor_absolute_position -= linear_actuator_step_size; |
| mdavis30 | 0:645ad86abcba | 268 | } |
| mdavis30 | 0:645ad86abcba | 269 | |
| mdavis30 | 0:645ad86abcba | 270 | MC.printf("la%d\r", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 271 | MC.printf("m\r"); |
| mdavis30 | 0:645ad86abcba | 272 | |
| mdavis30 | 0:645ad86abcba | 273 | char position[80]; |
| mdavis30 | 0:645ad86abcba | 274 | sprintf(position, "\nA received! (MBED) motor_absolute_position\nMBED_m_a_p: %d\n", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 275 | return position; |
| mdavis30 | 0:645ad86abcba | 276 | } |
| mdavis30 | 0:645ad86abcba | 277 | |
| mdavis30 | 1:ca2454ef80d9 | 278 | //EXTEND |
| mdavis30 | 0:645ad86abcba | 279 | string Battery_Linear_Actuator::Keyboard_D() |
| mdavis30 | 0:645ad86abcba | 280 | { |
| mdavis30 | 0:645ad86abcba | 281 | if (motor_absolute_position <= 175000) |
| mdavis30 | 0:645ad86abcba | 282 | { |
| mdavis30 | 0:645ad86abcba | 283 | //explicitly written, check if this will not go past 175000 |
| mdavis30 | 0:645ad86abcba | 284 | map_check = motor_absolute_position + linear_actuator_step_size; |
| mdavis30 | 0:645ad86abcba | 285 | if (map_check <= 175000) |
| mdavis30 | 0:645ad86abcba | 286 | motor_absolute_position += linear_actuator_step_size; |
| mdavis30 | 0:645ad86abcba | 287 | } |
| mdavis30 | 0:645ad86abcba | 288 | |
| mdavis30 | 0:645ad86abcba | 289 | MC.printf("la%d\r", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 290 | MC.printf("m\r"); |
| mdavis30 | 0:645ad86abcba | 291 | |
| mdavis30 | 0:645ad86abcba | 292 | char position[80]; |
| mdavis30 | 0:645ad86abcba | 293 | sprintf(position, "\nA received! (MBED) motor_absolute_position\nMBED_m_a_p: %d\n", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 294 | return position; |
| mdavis30 | 0:645ad86abcba | 295 | } |
| mdavis30 | 0:645ad86abcba | 296 | |
| mdavis30 | 0:645ad86abcba | 297 | //void Keyboard_P() |
| mdavis30 | 0:645ad86abcba | 298 | //{ |
| mdavis30 | 0:645ad86abcba | 299 | // //Buoyancy Engine gets about 14.5 inches of travel before it hits end of screw (inner piston) |
| mdavis30 | 0:645ad86abcba | 300 | // //max voltage on potentiometer is 3.06 volts |
| mdavis30 | 0:645ad86abcba | 301 | // //min voltage on potentiometer is 0.70 volts |
| mdavis30 | 0:645ad86abcba | 302 | // PC.printf("\nP received!\n"); |
| mdavis30 | 0:645ad86abcba | 303 | // float pot_check = potentiometer_analog_in.read() * 3.3; |
| mdavis30 | 0:645ad86abcba | 304 | // float pot_check_inches = 6.144 * pot_check - 4.3; |
| mdavis30 | 0:645ad86abcba | 305 | // float percent_travel_check = (pot_check_inches / 14.5) * 100; |
| mdavis30 | 0:645ad86abcba | 306 | // //PC.printf("(MBED) Potentiometer Reading: %f volts %f inches\n", pot_check, pot_check_inches); // Read the input voltage, represented as a float in the range [0.0, 1.0]. |
| mdavis30 | 0:645ad86abcba | 307 | //// PC.printf("Potentiometer Reading: %f volts %f inches %f%c\n ", pot_check, pot_check_inches, percent_travel_check, 37); |
| mdavis30 | 0:645ad86abcba | 308 | //// PC.printf("\n(MBED) Linear Actuator position: %d\n", motor_absolute_position); //wait(0.1); //Linear Actuator position |
| mdavis30 | 0:645ad86abcba | 309 | // |
| mdavis30 | 0:645ad86abcba | 310 | // PC.printf("B_E_POS: %d\n", pot_check); //wait(0.1); //Linear Actuator position |
| mdavis30 | 0:645ad86abcba | 311 | // PC.printf("MBED_m_a_p: %d \n", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 312 | // |
| mdavis30 | 0:645ad86abcba | 313 | // MC.printf("pos\r"); |
| mdavis30 | 0:645ad86abcba | 314 | // //MC.printf("pos\r"); wait(0.1); //position |
| mdavis30 | 0:645ad86abcba | 315 | // //get the next line from the Faulhaber Motor Controller in MATLAB? |
| mdavis30 | 0:645ad86abcba | 316 | //} |
| mdavis30 | 0:645ad86abcba | 317 | |
| mdavis30 | 0:645ad86abcba | 318 | //void BE_POSITION() |
| mdavis30 | 0:645ad86abcba | 319 | //{ |
| mdavis30 | 0:645ad86abcba | 320 | // float pot_check = potentiometer_analog_in.read() * 3.3; |
| mdavis30 | 0:645ad86abcba | 321 | // float pot_check_inches = 6.144 * pot_check - 4.3; |
| mdavis30 | 0:645ad86abcba | 322 | // float percent_travel_check = (pot_check_inches / 14.5) * 100; |
| mdavis30 | 0:645ad86abcba | 323 | // PC.printf("B_E_POS: %d\n", pot_check); //wait(0.1); //Linear Actuator position |
| mdavis30 | 0:645ad86abcba | 324 | //} |
| mdavis30 | 0:645ad86abcba | 325 | |
| mdavis30 | 0:645ad86abcba | 326 | // |
| mdavis30 | 0:645ad86abcba | 327 | //char ioc_char; //linear actuator i/o configuration |
| mdavis30 | 1:ca2454ef80d9 | 328 | //int ioc_array[28]; //http://stackoverflow.com/questions/439573/how-to-convert-a-single-char-into-an-int |
| mdavis30 | 1:ca2454ef80d9 | 329 | |
| mdavis30 | 1:ca2454ef80d9 | 330 | string Battery_Linear_Actuator::get_pos() |
| mdavis30 | 1:ca2454ef80d9 | 331 | { |
| mdavis30 | 1:ca2454ef80d9 | 332 | string pos_string = ""; |
| mdavis30 | 1:ca2454ef80d9 | 333 | |
| mdavis30 | 1:ca2454ef80d9 | 334 | MC.printf("pos\r"); |
| mdavis30 | 1:ca2454ef80d9 | 335 | //wait(0.1); //THIS SHOULD WORK! |
| mdavis30 | 1:ca2454ef80d9 | 336 | |
| mdavis30 | 1:ca2454ef80d9 | 337 | if(MC.readable()) //if you can read the motor controller do this... |
| mdavis30 | 1:ca2454ef80d9 | 338 | { |
| mdavis30 | 1:ca2454ef80d9 | 339 | while(MC.readable()) |
| mdavis30 | 1:ca2454ef80d9 | 340 | { |
| mdavis30 | 1:ca2454ef80d9 | 341 | pos_string += MC.getc(); |
| mdavis30 | 1:ca2454ef80d9 | 342 | wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing) |
| mdavis30 | 1:ca2454ef80d9 | 343 | } |
| mdavis30 | 1:ca2454ef80d9 | 344 | |
| mdavis30 | 1:ca2454ef80d9 | 345 | //MAKE THIS CLEANER 6/22/17 (FIX) |
| mdavis30 | 1:ca2454ef80d9 | 346 | //https://stackoverflow.com/questions/13294067/how-to-convert-string-to-char-array-in-c |
| mdavis30 | 1:ca2454ef80d9 | 347 | char pos_char[1024]; |
| mdavis30 | 1:ca2454ef80d9 | 348 | strcpy(pos_char, pos_string.c_str()); //copy it to the array? |
| mdavis30 | 1:ca2454ef80d9 | 349 | |
| mdavis30 | 1:ca2454ef80d9 | 350 | actual_position = atoi(pos_char); //convert char array to int (global variable) |
| mdavis30 | 1:ca2454ef80d9 | 351 | |
| mdavis30 | 1:ca2454ef80d9 | 352 | return pos_string; //return the character array as a string |
| mdavis30 | 1:ca2454ef80d9 | 353 | } |
| mdavis30 | 1:ca2454ef80d9 | 354 | |
| mdavis30 | 1:ca2454ef80d9 | 355 | else |
| mdavis30 | 1:ca2454ef80d9 | 356 | return "-999999"; //not working |
| mdavis30 | 1:ca2454ef80d9 | 357 | } |
| mdavis30 | 1:ca2454ef80d9 | 358 | |
| mdavis30 | 1:ca2454ef80d9 | 359 | string Battery_Linear_Actuator::PID_velocity_control(float la_setPoint, float IMU_pitch_angle, float la_P_gain, float la_I_gain, float la_D_gain) |
| mdavis30 | 1:ca2454ef80d9 | 360 | { |
| mdavis30 | 1:ca2454ef80d9 | 361 | lastTime = curr_time; //rename this later, old code |
| mdavis30 | 1:ca2454ef80d9 | 362 | curr_time = systemTime().read(); |
| mdavis30 | 1:ca2454ef80d9 | 363 | dt = curr_time-lastTime; |
| mdavis30 | 1:ca2454ef80d9 | 364 | |
| mdavis30 | 1:ca2454ef80d9 | 365 | //http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ |
| mdavis30 | 1:ca2454ef80d9 | 366 | |
| mdavis30 | 1:ca2454ef80d9 | 367 | //COMPUTE ERROR |
| mdavis30 | 1:ca2454ef80d9 | 368 | la_error = la_setPoint - IMU_pitch_angle; |
| mdavis30 | 1:ca2454ef80d9 | 369 | la_error_sum = la_error_sum + (la_error * dt); //INTEGRAL |
| mdavis30 | 1:ca2454ef80d9 | 370 | la_derivative_error = (la_error - lastErr) / dt; |
| mdavis30 | 1:ca2454ef80d9 | 371 | |
| mdavis30 | 1:ca2454ef80d9 | 372 | //COMPUTE PID OUTPUT |
| mdavis30 | 1:ca2454ef80d9 | 373 | la_output = la_P_gain*la_error + la_I_gain*la_error_sum + la_D_gain*la_derivative_error; |
| mdavis30 | 1:ca2454ef80d9 | 374 | //PC.printf("DEBUG: Linear Actuator Output: %3.3f\n", la_output); |
| mdavis30 | 1:ca2454ef80d9 | 375 | |
| mdavis30 | 1:ca2454ef80d9 | 376 | //RECORD LAST ITERATION OF VARIABLES |
| mdavis30 | 1:ca2454ef80d9 | 377 | lastErr = la_error; |
| mdavis30 | 1:ca2454ef80d9 | 378 | lastTime = curr_time; |
| mdavis30 | 1:ca2454ef80d9 | 379 | |
| mdavis30 | 1:ca2454ef80d9 | 380 | //DEBUG |
| mdavis30 | 1:ca2454ef80d9 | 381 | //PC.printf("DEBUG: IMU PITCH ANGLE: %3.3f\n", IMU_pitch_angle); |
| mdavis30 | 1:ca2454ef80d9 | 382 | //PC.printf("DEBUG: lastTime %3.3f, curr_time %3.3f, dt %3.3f\n", lastTime, curr_time, dt); |
| mdavis30 | 1:ca2454ef80d9 | 383 | // PC.printf("DEBUG: la_error %3.3f, la_error_sum %3.3f, la_derivative_error %3.3f\n", la_error, la_error_sum, la_derivative_error); |
| mdavis30 | 1:ca2454ef80d9 | 384 | // PC.printf("> DEBUG: la_output: %3.3f IMU PITCH ANGLE: %3.3f SETPOINT: %3.3f\n", la_output, IMU_pitch_angle, la_setPoint); |
| mdavis30 | 1:ca2454ef80d9 | 385 | // PC.printf("DEBUG: P:%3.3f I:%3.3f D:%3.3f\n", la_P_gain, la_I_gain, la_D_gain); |
| mdavis30 | 1:ca2454ef80d9 | 386 | |
| mdavis30 | 1:ca2454ef80d9 | 387 | char char_buffer [256]; |
| mdavis30 | 1:ca2454ef80d9 | 388 | string Battery_Position_String; |
| mdavis30 | 1:ca2454ef80d9 | 389 | // //DEBUG |
| mdavis30 | 1:ca2454ef80d9 | 390 | |
| mdavis30 | 1:ca2454ef80d9 | 391 | /******** Integer wind-up prevention based on Trent's code ***********/ |
| mdavis30 | 1:ca2454ef80d9 | 392 | if (la_output > 100) //change to 500 when working properly (500 rev/min) |
| mdavis30 | 1:ca2454ef80d9 | 393 | { |
| mdavis30 | 1:ca2454ef80d9 | 394 | la_output = 100.0; //limit the output to +100 |
| mdavis30 | 1:ca2454ef80d9 | 395 | la_error_sum = la_error_sum - (la_error * dt); |
| mdavis30 | 1:ca2454ef80d9 | 396 | //implement saturation instead of reset |
| mdavis30 | 1:ca2454ef80d9 | 397 | } |
| mdavis30 | 1:ca2454ef80d9 | 398 | else if (la_output < -100) |
| mdavis30 | 1:ca2454ef80d9 | 399 | { |
| mdavis30 | 1:ca2454ef80d9 | 400 | la_output = -100.0; //limit the output to -100 |
| mdavis30 | 1:ca2454ef80d9 | 401 | la_error_sum = la_error_sum + (la_error * dt); |
| mdavis30 | 1:ca2454ef80d9 | 402 | //implement saturation instead of reset |
| mdavis30 | 1:ca2454ef80d9 | 403 | } |
| mdavis30 | 1:ca2454ef80d9 | 404 | /******** Integer wind-up prevention based on Trent's code ***********/ |
| mdavis30 | 1:ca2454ef80d9 | 405 | |
| mdavis30 | 1:ca2454ef80d9 | 406 | //deadband in degrees, if error is less than X deg or greater than -X deg |
| mdavis30 | 1:ca2454ef80d9 | 407 | if (la_error < 4 && la_error > -4) |
| mdavis30 | 1:ca2454ef80d9 | 408 | { |
| mdavis30 | 1:ca2454ef80d9 | 409 | la_output = 0.0; |
| mdavis30 | 1:ca2454ef80d9 | 410 | } |
| mdavis30 | 1:ca2454ef80d9 | 411 | |
| mdavis30 | 1:ca2454ef80d9 | 412 | //send command to linear actuator motor controller |
| mdavis30 | 1:ca2454ef80d9 | 413 | //TROY: flip the output to get it to move in the right direction...? |
| mdavis30 | 1:ca2454ef80d9 | 414 | la_output = -la_output; |
| mdavis30 | 1:ca2454ef80d9 | 415 | //TROY: This seems to work well. 6/19/17 |
| mdavis30 | 1:ca2454ef80d9 | 416 | |
| mdavis30 | 1:ca2454ef80d9 | 417 | int integer_output = int(la_output); //output converted to integer for the controller, only uses integer inputs in rev/min |
| mdavis30 | 1:ca2454ef80d9 | 418 | Battery_Linear_Actuator::velocity_only(integer_output); |
| mdavis30 | 1:ca2454ef80d9 | 419 | |
| mdavis30 | 1:ca2454ef80d9 | 420 | //DEBUG |
| mdavis30 | 1:ca2454ef80d9 | 421 | sprintf(char_buffer, "DEBUG: la_error %3.3f, la_error_sum %3.3f, la_derivative_error %3.3f\n> DEBUG: la_output: %3.3f IMU PITCH ANGLE: %3.3f SETPOINT: %3.3f\n > integer_output: %d", la_error, la_error_sum, la_derivative_error, la_output, IMU_pitch_angle, la_setPoint, integer_output); |
| mdavis30 | 1:ca2454ef80d9 | 422 | return char_buffer; |
| mdavis30 | 1:ca2454ef80d9 | 423 | } |
| mdavis30 | 1:ca2454ef80d9 | 424 | //putting the functionality into the class |