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