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.
Fork of
Battery_Linear_Actuator_7_14
by 
Troy Holley
Battery_Linear_Actuator.cpp@3:9181d42863cd, 2017-07-21 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jul 21 13:29:09 2017 +0000
- Revision:
- 3:9181d42863cd
- Parent:
- 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 = ""; |
| tnhnrl | 2:627fd46c2b99 | 54 | wait(0.1); //THIS WORKS (based on testing)! |
| mdavis30 | 1:ca2454ef80d9 | 55 | |
| tnhnrl | 2:627fd46c2b99 | 56 | if(MC.readable()) //if you can read the LA motor controller do this... |
| tnhnrl | 2:627fd46c2b99 | 57 | { |
| mdavis30 | 1:ca2454ef80d9 | 58 | while(MC.readable()) |
| tnhnrl | 2:627fd46c2b99 | 59 | { |
| mdavis30 | 1:ca2454ef80d9 | 60 | bla_string += MC.getc(); |
| mdavis30 | 1:ca2454ef80d9 | 61 | wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing) |
| mdavis30 | 1:ca2454ef80d9 | 62 | } |
| tnhnrl | 2:627fd46c2b99 | 63 | |
| tnhnrl | 2:627fd46c2b99 | 64 | return bla_string; //return the character array as a string |
| mdavis30 | 1:ca2454ef80d9 | 65 | } |
| tnhnrl | 2:627fd46c2b99 | 66 | else |
| tnhnrl | 2:627fd46c2b99 | 67 | { |
| tnhnrl | 2:627fd46c2b99 | 68 | return ""; //return the empty string because you didn't read anything |
| tnhnrl | 2:627fd46c2b99 | 69 | } |
| mdavis30 | 1:ca2454ef80d9 | 70 | } |
| mdavis30 | 1:ca2454ef80d9 | 71 | |
| mdavis30 | 0:645ad86abcba | 72 | string Battery_Linear_Actuator::Keyboard_O() |
| mdavis30 | 0:645ad86abcba | 73 | { |
| mdavis30 | 0:645ad86abcba | 74 | //PC.printf("MBED_m_a_p: %d \n", motor_absolute_position); //linear actuator position |
| mdavis30 | 0:645ad86abcba | 75 | |
| mdavis30 | 0:645ad86abcba | 76 | MC.printf("pos\r"); //get position from motor controller...maybe rewrite this |
| mdavis30 | 0:645ad86abcba | 77 | //get this from the Faulhaber Motor Controller in MATLAB? |
| mdavis30 | 0:645ad86abcba | 78 | |
| mdavis30 | 0:645ad86abcba | 79 | char char_buffer [60]; |
| mdavis30 | 0:645ad86abcba | 80 | string Battery_Position_String; |
| mdavis30 | 0:645ad86abcba | 81 | sprintf(char_buffer,"MBED_m_a_p: %d \n", motor_absolute_position); //linear actuator position |
| mdavis30 | 0:645ad86abcba | 82 | Battery_Position_String = char_buffer; |
| mdavis30 | 0:645ad86abcba | 83 | return Battery_Position_String; |
| mdavis30 | 0:645ad86abcba | 84 | } |
| mdavis30 | 0:645ad86abcba | 85 | |
| mdavis30 | 0:645ad86abcba | 86 | string Battery_Linear_Actuator::Keyboard_H() //Home linear actuator |
| mdavis30 | 0:645ad86abcba | 87 | { |
| mdavis30 | 0:645ad86abcba | 88 | //reset position in program: |
| mdavis30 | 0:645ad86abcba | 89 | motor_absolute_position = 0; |
| mdavis30 | 0:645ad86abcba | 90 | |
| mdavis30 | 0:645ad86abcba | 91 | MC.printf("hosp-100\r"); wait(0.1); //homing speed -100 (verified |
| mdavis30 | 0:645ad86abcba | 92 | MC.printf("ghosp\r"); wait(0.1);//check homing speed |
| mdavis30 | 0:645ad86abcba | 93 | MC.printf("SHL1\r"); wait(0.1);//setup fault and AnIn |
| mdavis30 | 0:645ad86abcba | 94 | MC.printf("gohoseq\r"); wait(0.1);//enable the sequence |
| mdavis30 | 0:645ad86abcba | 95 | |
| mdavis30 | 0:645ad86abcba | 96 | char homing_string[80]; |
| mdavis30 | 0:645ad86abcba | 97 | sprintf(homing_string,"\nH received! (Homing Sequence In Progress)\nMBED_m_a_p: %d \n", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 98 | return homing_string; |
| mdavis30 | 0:645ad86abcba | 99 | } |
| mdavis30 | 0:645ad86abcba | 100 | |
| mdavis30 | 1:ca2454ef80d9 | 101 | string Battery_Linear_Actuator::Keyboard_J() //L.A. POSITION ONLY |
| mdavis30 | 1:ca2454ef80d9 | 102 | { |
| mdavis30 | 1:ca2454ef80d9 | 103 | MC.printf("pos\r"); |
| mdavis30 | 1:ca2454ef80d9 | 104 | return "\nL.A. POSITION COMMAND RECEIVED.\n"; |
| mdavis30 | 1:ca2454ef80d9 | 105 | } |
| mdavis30 | 1:ca2454ef80d9 | 106 | |
| mdavis30 | 1:ca2454ef80d9 | 107 | string Battery_Linear_Actuator::Velocity(int velocity) //EXTEND with velocity command |
| mdavis30 | 1:ca2454ef80d9 | 108 | { |
| mdavis30 | 1:ca2454ef80d9 | 109 | string la_position = Battery_Linear_Actuator::get_pos(); //get the position data in string format |
| mdavis30 | 1:ca2454ef80d9 | 110 | |
| mdavis30 | 1:ca2454ef80d9 | 111 | //https://stackoverflow.com/questions/13294067/how-to-convert-string-to-char-array-in-c |
| mdavis30 | 1:ca2454ef80d9 | 112 | char la_position_char[1024]; |
| mdavis30 | 1:ca2454ef80d9 | 113 | strcpy(la_position_char, la_position.c_str()); //copy it to the array? |
| mdavis30 | 1:ca2454ef80d9 | 114 | |
| mdavis30 | 1:ca2454ef80d9 | 115 | //back to the drawing board |
| mdavis30 | 1:ca2454ef80d9 | 116 | //https://developer.mbed.org/questions/2910/Convert-string-to-int/ |
| mdavis30 | 1:ca2454ef80d9 | 117 | |
| mdavis30 | 1:ca2454ef80d9 | 118 | int int_position = atoi(la_position_char); |
| mdavis30 | 1:ca2454ef80d9 | 119 | |
| mdavis30 | 1:ca2454ef80d9 | 120 | if (int_position >= 0 && int_position <= 175000) |
| mdavis30 | 1:ca2454ef80d9 | 121 | { |
| mdavis30 | 1:ca2454ef80d9 | 122 | MC.printf("en\r"); |
| mdavis30 | 1:ca2454ef80d9 | 123 | MC.printf("V%d\r", velocity); |
| mdavis30 | 1:ca2454ef80d9 | 124 | |
| mdavis30 | 1:ca2454ef80d9 | 125 | char velocity_string[80]; |
| tnhnrl | 2:627fd46c2b99 | 126 | sprintf(velocity_string, "\nVELOCITY COMMAND: V%d\n", velocity); |
| mdavis30 | 1:ca2454ef80d9 | 127 | |
| mdavis30 | 1:ca2454ef80d9 | 128 | return velocity_string; |
| mdavis30 | 1:ca2454ef80d9 | 129 | } |
| mdavis30 | 1:ca2454ef80d9 | 130 | |
| mdavis30 | 1:ca2454ef80d9 | 131 | else |
| mdavis30 | 1:ca2454ef80d9 | 132 | return "outside of bounds"; |
| mdavis30 | 1:ca2454ef80d9 | 133 | } |
| mdavis30 | 1:ca2454ef80d9 | 134 | |
| mdavis30 | 1:ca2454ef80d9 | 135 | string Battery_Linear_Actuator::velocity_only(int velocity) //EXTEND with velocity command |
| mdavis30 | 1:ca2454ef80d9 | 136 | { |
| mdavis30 | 1:ca2454ef80d9 | 137 | //this uses the position record saved in the object |
| mdavis30 | 1:ca2454ef80d9 | 138 | if (actual_position >= 0 && actual_position <= 175000) |
| mdavis30 | 1:ca2454ef80d9 | 139 | { |
| tnhnrl | 2:627fd46c2b99 | 140 | //send velocity command and get the return value |
| mdavis30 | 1:ca2454ef80d9 | 141 | MC.printf("en\r"); |
| mdavis30 | 1:ca2454ef80d9 | 142 | MC.printf("V%d\r", velocity); |
| tnhnrl | 2:627fd46c2b99 | 143 | MC.printf("GV\r", velocity); |
| tnhnrl | 2:627fd46c2b99 | 144 | string velocity_return_string = Battery_Linear_Actuator::MC_readable_redux(); |
| mdavis30 | 1:ca2454ef80d9 | 145 | |
| tnhnrl | 2:627fd46c2b99 | 146 | char velocity_string[80]; |
| tnhnrl | 2:627fd46c2b99 | 147 | sprintf(velocity_string, "\nVELOCITY COMMAND: V%d\nRETURN VELOCITY gv%s\n", velocity, velocity_return_string.c_str()); |
| tnhnrl | 2:627fd46c2b99 | 148 | |
| mdavis30 | 1:ca2454ef80d9 | 149 | return velocity_string; |
| mdavis30 | 1:ca2454ef80d9 | 150 | } |
| mdavis30 | 1:ca2454ef80d9 | 151 | |
| mdavis30 | 1:ca2454ef80d9 | 152 | else |
| mdavis30 | 1:ca2454ef80d9 | 153 | return "outside of bounds"; //maybe change this |
| mdavis30 | 1:ca2454ef80d9 | 154 | } |
| mdavis30 | 1:ca2454ef80d9 | 155 | |
| mdavis30 | 1:ca2454ef80d9 | 156 | string Battery_Linear_Actuator::Keyboard_K() //EXTEND with velocity command |
| mdavis30 | 1:ca2454ef80d9 | 157 | { |
| mdavis30 | 1:ca2454ef80d9 | 158 | if (motor_absolute_position >= 0 && motor_absolute_position <= 175000) |
| mdavis30 | 1:ca2454ef80d9 | 159 | { |
| mdavis30 | 1:ca2454ef80d9 | 160 | MC.printf("en\r"); |
| mdavis30 | 1:ca2454ef80d9 | 161 | MC.printf("V50\r"); |
| mdavis30 | 1:ca2454ef80d9 | 162 | return "\nVELOCITY COMMAND. Extending.\n"; |
| mdavis30 | 1:ca2454ef80d9 | 163 | } |
| mdavis30 | 1:ca2454ef80d9 | 164 | else |
| mdavis30 | 1:ca2454ef80d9 | 165 | return "\nmotor position out of bounds (below 0 or above 175000)\n"; |
| mdavis30 | 1:ca2454ef80d9 | 166 | } |
| mdavis30 | 1:ca2454ef80d9 | 167 | |
| mdavis30 | 1:ca2454ef80d9 | 168 | string Battery_Linear_Actuator::Keyboard_L() //RETRACT with velocity command |
| mdavis30 | 1:ca2454ef80d9 | 169 | { |
| mdavis30 | 1:ca2454ef80d9 | 170 | if (motor_absolute_position >= 0 && motor_absolute_position <= 175000) |
| mdavis30 | 1:ca2454ef80d9 | 171 | { |
| mdavis30 | 1:ca2454ef80d9 | 172 | MC.printf("en\r"); |
| mdavis30 | 1:ca2454ef80d9 | 173 | MC.printf("V-50\r"); |
| mdavis30 | 1:ca2454ef80d9 | 174 | return "\nVELOCITY COMMAND. Retracting.\n"; |
| mdavis30 | 1:ca2454ef80d9 | 175 | } |
| mdavis30 | 1:ca2454ef80d9 | 176 | else |
| mdavis30 | 1:ca2454ef80d9 | 177 | return "\nmotor position out of bounds (below 0 or above 175000)\n"; |
| mdavis30 | 1:ca2454ef80d9 | 178 | } |
| mdavis30 | 1:ca2454ef80d9 | 179 | string Battery_Linear_Actuator::Keyboard_U() |
| mdavis30 | 1:ca2454ef80d9 | 180 | { |
| mdavis30 | 1:ca2454ef80d9 | 181 | MC.printf("v0\r"); //velocity 0 |
| mdavis30 | 1:ca2454ef80d9 | 182 | MC.printf("di\r"); //motor disabled |
| mdavis30 | 1:ca2454ef80d9 | 183 | return "\nVelocity=0, L.A. motor disabled.\n"; |
| mdavis30 | 1:ca2454ef80d9 | 184 | } |
| mdavis30 | 1:ca2454ef80d9 | 185 | |
| mdavis30 | 0:645ad86abcba | 186 | string Battery_Linear_Actuator::Keyboard_E() //enable linear actuator |
| mdavis30 | 0:645ad86abcba | 187 | { |
| mdavis30 | 0:645ad86abcba | 188 | MC.printf("en\r"); |
| mdavis30 | 0:645ad86abcba | 189 | return "\nE received!\nMOTOR ENABLED!"; |
| mdavis30 | 0:645ad86abcba | 190 | } |
| mdavis30 | 0:645ad86abcba | 191 | |
| mdavis30 | 0:645ad86abcba | 192 | string Battery_Linear_Actuator::Keyboard_Q() //disable linear actuator |
| mdavis30 | 0:645ad86abcba | 193 | { |
| mdavis30 | 0:645ad86abcba | 194 | MC.printf("di\r"); |
| mdavis30 | 0:645ad86abcba | 195 | return "\nQ received!\nMOTOR DISABLED!"; |
| mdavis30 | 0:645ad86abcba | 196 | } |
| mdavis30 | 0:645ad86abcba | 197 | |
| mdavis30 | 0:645ad86abcba | 198 | string Battery_Linear_Actuator::Keyboard_EQUAL_KEY() //INCREASE L.A. step size |
| mdavis30 | 0:645ad86abcba | 199 | { |
| mdavis30 | 0:645ad86abcba | 200 | if (linear_actuator_step_size <= 9800) //arbitrary stop at 10000 |
| mdavis30 | 0:645ad86abcba | 201 | linear_actuator_step_size += 200; |
| mdavis30 | 0:645ad86abcba | 202 | char bla_string[80]; |
| tnhnrl | 2:627fd46c2b99 | 203 | sprintf(bla_string, "\n(MBED) Linear Actuator step size INCREASED\nLAM_SZ: %d", linear_actuator_step_size); |
| mdavis30 | 0:645ad86abcba | 204 | |
| mdavis30 | 0:645ad86abcba | 205 | return bla_string; |
| mdavis30 | 0:645ad86abcba | 206 | } |
| mdavis30 | 0:645ad86abcba | 207 | |
| mdavis30 | 0:645ad86abcba | 208 | string Battery_Linear_Actuator::Keyboard_DASH_KEY() //DECREASE L.A. step size |
| mdavis30 | 0:645ad86abcba | 209 | { |
| mdavis30 | 0:645ad86abcba | 210 | if (linear_actuator_step_size >= 200) |
| mdavis30 | 0:645ad86abcba | 211 | linear_actuator_step_size -= 200; |
| mdavis30 | 0:645ad86abcba | 212 | char bla_string[80]; |
| tnhnrl | 2:627fd46c2b99 | 213 | sprintf(bla_string, "\n(MBED) Linear Actuator step size DECREASED\nLAM_SZ: %d", linear_actuator_step_size); |
| mdavis30 | 0:645ad86abcba | 214 | |
| mdavis30 | 0:645ad86abcba | 215 | return bla_string; |
| mdavis30 | 0:645ad86abcba | 216 | } |
| mdavis30 | 0:645ad86abcba | 217 | |
| mdavis30 | 0:645ad86abcba | 218 | string Battery_Linear_Actuator::Keyboard_LEFT_BRACKET() |
| mdavis30 | 0:645ad86abcba | 219 | { //100 to 30000, DECREASE (100 increments) |
| mdavis30 | 1:ca2454ef80d9 | 220 | if (linear_actuator_motor_speed >= 200) |
| mdavis30 | 1:ca2454ef80d9 | 221 | linear_actuator_motor_speed -= 200; |
| mdavis30 | 0:645ad86abcba | 222 | //PC.printf("\n(MBED) Linear Actuator Motor Speed INCREASED: sp%d\n", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 223 | MC.printf("sp%d\r", linear_actuator_motor_speed); //retesting linear actuator motor speed 5/11/17 |
| mdavis30 | 0:645ad86abcba | 224 | //PC.printf("\nLAMSP: %d\n", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 225 | |
| mdavis30 | 0:645ad86abcba | 226 | char str[80]; |
| mdavis30 | 0:645ad86abcba | 227 | sprintf(str,"(MBED) Linear Actuator Motor Speed DECREASED\nLA_SP: %d", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 228 | |
| mdavis30 | 0:645ad86abcba | 229 | return str; |
| mdavis30 | 0:645ad86abcba | 230 | } |
| mdavis30 | 0:645ad86abcba | 231 | |
| mdavis30 | 0:645ad86abcba | 232 | string Battery_Linear_Actuator::Keyboard_RIGHT_BRACKET() //disable linear actuator |
| mdavis30 | 0:645ad86abcba | 233 | { //100 to 30000, INCREASE (100 increments) |
| mdavis30 | 1:ca2454ef80d9 | 234 | if (linear_actuator_motor_speed <= 29800) |
| mdavis30 | 1:ca2454ef80d9 | 235 | linear_actuator_motor_speed += 200; |
| mdavis30 | 0:645ad86abcba | 236 | //PC.printf("\n(MBED) Linear Actuator Motor Speed INCREASED: sp%d\n", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 237 | MC.printf("sp%d\r", linear_actuator_motor_speed); //retesting linear actuator motor speed 5/11/17 |
| mdavis30 | 0:645ad86abcba | 238 | //PC.printf("\nLAMSP: %d\n", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 239 | |
| mdavis30 | 0:645ad86abcba | 240 | char str[80]; |
| mdavis30 | 0:645ad86abcba | 241 | sprintf(str,"(MBED) Linear Actuator Motor Speed INCREASED\nLA_SP: %d", linear_actuator_motor_speed); |
| mdavis30 | 0:645ad86abcba | 242 | |
| mdavis30 | 0:645ad86abcba | 243 | return str; |
| mdavis30 | 0:645ad86abcba | 244 | } |
| mdavis30 | 0:645ad86abcba | 245 | |
| mdavis30 | 0:645ad86abcba | 246 | //http://stackoverflow.com/questions/8011700/how-do-i-extract-specific-n-bits-of-a-32-bit-unsigned-integer-in-c |
| mdavis30 | 0:645ad86abcba | 247 | unsigned createMask(unsigned a, unsigned b) { |
| mdavis30 | 0:645ad86abcba | 248 | unsigned r = 0; |
| mdavis30 | 0:645ad86abcba | 249 | for (unsigned i=a; i<=b; i++) |
| mdavis30 | 0:645ad86abcba | 250 | r |= 1 << i; |
| mdavis30 | 0:645ad86abcba | 251 | |
| mdavis30 | 0:645ad86abcba | 252 | return r; |
| mdavis30 | 0:645ad86abcba | 253 | } |
| mdavis30 | 0:645ad86abcba | 254 | |
| mdavis30 | 1:ca2454ef80d9 | 255 | //RETRACT |
| mdavis30 | 0:645ad86abcba | 256 | string Battery_Linear_Actuator::Keyboard_A() |
| mdavis30 | 0:645ad86abcba | 257 | { |
| mdavis30 | 1:ca2454ef80d9 | 258 | // float time_read = systemTime().read(); //added #include "StaticDefs.hpp" on 6/14/17 |
| mdavis30 | 1:ca2454ef80d9 | 259 | // char position[80]; |
| mdavis30 | 1:ca2454ef80d9 | 260 | // sprintf(position, "\nSYSTEM TIME IS %f\n", time_read); //SYSTEM TIME IS 24.918480 |
| mdavis30 | 1:ca2454ef80d9 | 261 | // return position; |
| mdavis30 | 1:ca2454ef80d9 | 262 | |
| mdavis30 | 0:645ad86abcba | 263 | if (motor_absolute_position >= 0) |
| mdavis30 | 0:645ad86abcba | 264 | { |
| mdavis30 | 0:645ad86abcba | 265 | //explicitly written, check if this will not go past 175000 |
| mdavis30 | 0:645ad86abcba | 266 | map_check = motor_absolute_position - linear_actuator_step_size; |
| mdavis30 | 0:645ad86abcba | 267 | if (map_check >= 0) |
| mdavis30 | 0:645ad86abcba | 268 | motor_absolute_position -= linear_actuator_step_size; |
| mdavis30 | 0:645ad86abcba | 269 | } |
| mdavis30 | 0:645ad86abcba | 270 | |
| mdavis30 | 0:645ad86abcba | 271 | MC.printf("la%d\r", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 272 | MC.printf("m\r"); |
| mdavis30 | 0:645ad86abcba | 273 | |
| mdavis30 | 0:645ad86abcba | 274 | char position[80]; |
| tnhnrl | 2:627fd46c2b99 | 275 | //sprintf(position, "\nA received! (MBED) motor_absolute_position\nMBED_m_a_p: %d\n", motor_absolute_position); |
| tnhnrl | 2:627fd46c2b99 | 276 | sprintf(position, "\nA received! (MBED) motor_absolute_position\nLAM_POS: %d\n", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 277 | return position; |
| mdavis30 | 0:645ad86abcba | 278 | } |
| mdavis30 | 0:645ad86abcba | 279 | |
| mdavis30 | 1:ca2454ef80d9 | 280 | //EXTEND |
| mdavis30 | 0:645ad86abcba | 281 | string Battery_Linear_Actuator::Keyboard_D() |
| mdavis30 | 0:645ad86abcba | 282 | { |
| mdavis30 | 0:645ad86abcba | 283 | if (motor_absolute_position <= 175000) |
| mdavis30 | 0:645ad86abcba | 284 | { |
| mdavis30 | 0:645ad86abcba | 285 | //explicitly written, check if this will not go past 175000 |
| mdavis30 | 0:645ad86abcba | 286 | map_check = motor_absolute_position + linear_actuator_step_size; |
| mdavis30 | 0:645ad86abcba | 287 | if (map_check <= 175000) |
| mdavis30 | 0:645ad86abcba | 288 | motor_absolute_position += linear_actuator_step_size; |
| mdavis30 | 0:645ad86abcba | 289 | } |
| mdavis30 | 0:645ad86abcba | 290 | |
| mdavis30 | 0:645ad86abcba | 291 | MC.printf("la%d\r", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 292 | MC.printf("m\r"); |
| mdavis30 | 0:645ad86abcba | 293 | |
| mdavis30 | 0:645ad86abcba | 294 | char position[80]; |
| tnhnrl | 2:627fd46c2b99 | 295 | //sprintf(position, "\nA received! (MBED) motor_absolute_position\nMBED_m_a_p: %d\n", motor_absolute_position); |
| tnhnrl | 2:627fd46c2b99 | 296 | sprintf(position, "\nA received! (MBED) motor_absolute_position\nLAM_POS: %d\n", motor_absolute_position); |
| mdavis30 | 0:645ad86abcba | 297 | return position; |
| mdavis30 | 0:645ad86abcba | 298 | } |
| mdavis30 | 0:645ad86abcba | 299 | |
| tnhnrl | 2:627fd46c2b99 | 300 | string Battery_Linear_Actuator::get_vel() |
| tnhnrl | 2:627fd46c2b99 | 301 | { |
| tnhnrl | 2:627fd46c2b99 | 302 | string vel_string = ""; |
| tnhnrl | 2:627fd46c2b99 | 303 | |
| tnhnrl | 2:627fd46c2b99 | 304 | MC.printf("gv\r"); |
| tnhnrl | 2:627fd46c2b99 | 305 | wait(0.1); //THIS SHOULD WORK! |
| tnhnrl | 2:627fd46c2b99 | 306 | |
| tnhnrl | 2:627fd46c2b99 | 307 | vel_string = Battery_Linear_Actuator::MC_readable_redux(); |
| tnhnrl | 2:627fd46c2b99 | 308 | return vel_string; |
| tnhnrl | 2:627fd46c2b99 | 309 | } |
| tnhnrl | 2:627fd46c2b99 | 310 | |
| mdavis30 | 1:ca2454ef80d9 | 311 | string Battery_Linear_Actuator::get_pos() |
| mdavis30 | 1:ca2454ef80d9 | 312 | { |
| mdavis30 | 1:ca2454ef80d9 | 313 | string pos_string = ""; |
| mdavis30 | 1:ca2454ef80d9 | 314 | |
| mdavis30 | 1:ca2454ef80d9 | 315 | MC.printf("pos\r"); |
| mdavis30 | 1:ca2454ef80d9 | 316 | |
| mdavis30 | 3:9181d42863cd | 317 | pos_string = Battery_Linear_Actuator::MC_readable_redux(); |
| mdavis30 | 3:9181d42863cd | 318 | return pos_string; |
| mdavis30 | 1:ca2454ef80d9 | 319 | } |
| mdavis30 | 1:ca2454ef80d9 | 320 | |
| mdavis30 | 1:ca2454ef80d9 | 321 | 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 | 322 | { |
| mdavis30 | 1:ca2454ef80d9 | 323 | lastTime = curr_time; //rename this later, old code |
| mdavis30 | 1:ca2454ef80d9 | 324 | curr_time = systemTime().read(); |
| mdavis30 | 1:ca2454ef80d9 | 325 | dt = curr_time-lastTime; |
| mdavis30 | 1:ca2454ef80d9 | 326 | |
| mdavis30 | 1:ca2454ef80d9 | 327 | //http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/ |
| mdavis30 | 1:ca2454ef80d9 | 328 | |
| mdavis30 | 1:ca2454ef80d9 | 329 | //COMPUTE ERROR |
| mdavis30 | 1:ca2454ef80d9 | 330 | la_error = la_setPoint - IMU_pitch_angle; |
| mdavis30 | 1:ca2454ef80d9 | 331 | la_error_sum = la_error_sum + (la_error * dt); //INTEGRAL |
| mdavis30 | 1:ca2454ef80d9 | 332 | la_derivative_error = (la_error - lastErr) / dt; |
| mdavis30 | 1:ca2454ef80d9 | 333 | |
| mdavis30 | 1:ca2454ef80d9 | 334 | //COMPUTE PID OUTPUT |
| mdavis30 | 1:ca2454ef80d9 | 335 | la_output = la_P_gain*la_error + la_I_gain*la_error_sum + la_D_gain*la_derivative_error; |
| mdavis30 | 1:ca2454ef80d9 | 336 | //PC.printf("DEBUG: Linear Actuator Output: %3.3f\n", la_output); |
| mdavis30 | 1:ca2454ef80d9 | 337 | |
| mdavis30 | 1:ca2454ef80d9 | 338 | //RECORD LAST ITERATION OF VARIABLES |
| mdavis30 | 1:ca2454ef80d9 | 339 | lastErr = la_error; |
| mdavis30 | 1:ca2454ef80d9 | 340 | lastTime = curr_time; |
| mdavis30 | 1:ca2454ef80d9 | 341 | |
| mdavis30 | 1:ca2454ef80d9 | 342 | char char_buffer [256]; |
| mdavis30 | 1:ca2454ef80d9 | 343 | string Battery_Position_String; |
| mdavis30 | 1:ca2454ef80d9 | 344 | |
| mdavis30 | 1:ca2454ef80d9 | 345 | /******** Integer wind-up prevention based on Trent's code ***********/ |
| mdavis30 | 1:ca2454ef80d9 | 346 | if (la_output > 100) //change to 500 when working properly (500 rev/min) |
| mdavis30 | 1:ca2454ef80d9 | 347 | { |
| mdavis30 | 1:ca2454ef80d9 | 348 | la_output = 100.0; //limit the output to +100 |
| mdavis30 | 1:ca2454ef80d9 | 349 | la_error_sum = la_error_sum - (la_error * dt); |
| mdavis30 | 1:ca2454ef80d9 | 350 | //implement saturation instead of reset |
| mdavis30 | 1:ca2454ef80d9 | 351 | } |
| mdavis30 | 1:ca2454ef80d9 | 352 | else if (la_output < -100) |
| mdavis30 | 1:ca2454ef80d9 | 353 | { |
| mdavis30 | 1:ca2454ef80d9 | 354 | la_output = -100.0; //limit the output to -100 |
| mdavis30 | 1:ca2454ef80d9 | 355 | la_error_sum = la_error_sum + (la_error * dt); |
| mdavis30 | 1:ca2454ef80d9 | 356 | //implement saturation instead of reset |
| mdavis30 | 1:ca2454ef80d9 | 357 | } |
| mdavis30 | 1:ca2454ef80d9 | 358 | /******** Integer wind-up prevention based on Trent's code ***********/ |
| mdavis30 | 1:ca2454ef80d9 | 359 | |
| mdavis30 | 1:ca2454ef80d9 | 360 | //deadband in degrees, if error is less than X deg or greater than -X deg |
| mdavis30 | 1:ca2454ef80d9 | 361 | if (la_error < 4 && la_error > -4) |
| mdavis30 | 1:ca2454ef80d9 | 362 | { |
| mdavis30 | 1:ca2454ef80d9 | 363 | la_output = 0.0; |
| mdavis30 | 1:ca2454ef80d9 | 364 | } |
| mdavis30 | 1:ca2454ef80d9 | 365 | |
| mdavis30 | 1:ca2454ef80d9 | 366 | //send command to linear actuator motor controller |
| mdavis30 | 3:9181d42863cd | 367 | //TROY: flip the output to get it to move in the right direction... |
| mdavis30 | 1:ca2454ef80d9 | 368 | la_output = -la_output; |
| mdavis30 | 1:ca2454ef80d9 | 369 | |
| mdavis30 | 1:ca2454ef80d9 | 370 | int integer_output = int(la_output); //output converted to integer for the controller, only uses integer inputs in rev/min |
| mdavis30 | 3:9181d42863cd | 371 | |
| mdavis30 | 3:9181d42863cd | 372 | //get linear actuator position |
| mdavis30 | 3:9181d42863cd | 373 | const char *char_actual_position = Battery_Linear_Actuator::get_pos().c_str(); |
| mdavis30 | 3:9181d42863cd | 374 | //actual_position_string = BLA_object.get_pos().c_str(); |
| mdavis30 | 3:9181d42863cd | 375 | |
| mdavis30 | 3:9181d42863cd | 376 | double double_actual_position = 0.0; |
| mdavis30 | 3:9181d42863cd | 377 | sscanf(char_actual_position, "%lf", &double_actual_position); //convert to double |
| mdavis30 | 3:9181d42863cd | 378 | |
| mdavis30 | 3:9181d42863cd | 379 | //IF THE POSITION IS BETWEEN 0 and 175,000 only, then send the velocity |
| mdavis30 | 3:9181d42863cd | 380 | string velocity_only_string = "velocity not sent?"; |
| mdavis30 | 1:ca2454ef80d9 | 381 | |
| mdavis30 | 3:9181d42863cd | 382 | //found this out through testing |
| mdavis30 | 3:9181d42863cd | 383 | //if velocity is positive you extend the nut |
| mdavis30 | 3:9181d42863cd | 384 | //but you don't want it to extend past the limit of 175,000 |
| mdavis30 | 3:9181d42863cd | 385 | if ((integer_output > 0) && (double_actual_position <= 180000.0)) |
| mdavis30 | 3:9181d42863cd | 386 | string velocity_only_string = Battery_Linear_Actuator::velocity_only(integer_output); //send the velocity command if this condition is true |
| mdavis30 | 3:9181d42863cd | 387 | else if ((integer_output < 0) && (double_actual_position >= 3000.0)) //it's a little jumpy (so go here instead of zero) |
| mdavis30 | 3:9181d42863cd | 388 | string velocity_only_string = Battery_Linear_Actuator::velocity_only(integer_output); //send the velocity command if this condition is true |
| mdavis30 | 3:9181d42863cd | 389 | else //conditions where we go past the limit |
| mdavis30 | 3:9181d42863cd | 390 | string velocity_only_string = Battery_Linear_Actuator::velocity_only(0); //send the velocity command if this condition is true |
| mdavis30 | 3:9181d42863cd | 391 | |
| mdavis30 | 3:9181d42863cd | 392 | //use this printout to debug it |
| mdavis30 | 3:9181d42863cd | 393 | sprintf(char_buffer, "What is the position from the PID loop: %lf\n", double_actual_position); |
| mdavis30 | 3:9181d42863cd | 394 | //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\n%s\n", la_error, la_error_sum, la_derivative_error, la_output, IMU_pitch_angle, la_setPoint, integer_output, velocity_only_string); |
| mdavis30 | 1:ca2454ef80d9 | 395 | return char_buffer; |
| mdavis30 | 1:ca2454ef80d9 | 396 | } |
| mdavis30 | 1:ca2454ef80d9 | 397 | //putting the functionality into the class |