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@3:9181d42863cd, 2017-07-21 (annotated)
- Committer:
- mdavis30
- Date:
- Fri Jul 21 13:29:09 2017 +0000
- Revision:
- 3:9181d42863cd
- Parent:
- 2:627fd46c2b99
- Child:
- 4:346911ee5450
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 |