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@2:627fd46c2b99, 2017-07-17 (annotated)

Committer:
tnhnrl
Date:
Mon Jul 17 14:23:34 2017 +0000
Revision:
2:627fd46c2b99
Parent:
1:ca2454ef80d9
Child:
3:9181d42863cd
7_14_17 fork

Who changed what in which revision?

UserRevisionLine numberNew 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
mdavis30 0:645ad86abcba 300 //void Keyboard_P()
mdavis30 0:645ad86abcba 301 //{
mdavis30 0:645ad86abcba 302 // //Buoyancy Engine gets about 14.5 inches of travel before it hits end of screw (inner piston)
mdavis30 0:645ad86abcba 303 // //max voltage on potentiometer is 3.06 volts
mdavis30 0:645ad86abcba 304 // //min voltage on potentiometer is 0.70 volts
mdavis30 0:645ad86abcba 305 // PC.printf("\nP received!\n");
mdavis30 0:645ad86abcba 306 // float pot_check = potentiometer_analog_in.read() * 3.3;
mdavis30 0:645ad86abcba 307 // float pot_check_inches = 6.144 * pot_check - 4.3;
mdavis30 0:645ad86abcba 308 // float percent_travel_check = (pot_check_inches / 14.5) * 100;
mdavis30 0:645ad86abcba 309 // //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 310 //// PC.printf("Potentiometer Reading: %f volts %f inches %f%c\n ", pot_check, pot_check_inches, percent_travel_check, 37);
mdavis30 0:645ad86abcba 311 //// PC.printf("\n(MBED) Linear Actuator position: %d\n", motor_absolute_position); //wait(0.1); //Linear Actuator position
mdavis30 0:645ad86abcba 312 //
mdavis30 0:645ad86abcba 313 // PC.printf("B_E_POS: %d\n", pot_check); //wait(0.1); //Linear Actuator position
mdavis30 0:645ad86abcba 314 // PC.printf("MBED_m_a_p: %d \n", motor_absolute_position);
mdavis30 0:645ad86abcba 315 //
mdavis30 0:645ad86abcba 316 // MC.printf("pos\r");
mdavis30 0:645ad86abcba 317 // //MC.printf("pos\r"); wait(0.1); //position
mdavis30 0:645ad86abcba 318 // //get the next line from the Faulhaber Motor Controller in MATLAB?
mdavis30 0:645ad86abcba 319 //}
mdavis30 0:645ad86abcba 320
mdavis30 0:645ad86abcba 321 //void BE_POSITION()
mdavis30 0:645ad86abcba 322 //{
mdavis30 0:645ad86abcba 323 // float pot_check = potentiometer_analog_in.read() * 3.3;
mdavis30 0:645ad86abcba 324 // float pot_check_inches = 6.144 * pot_check - 4.3;
mdavis30 0:645ad86abcba 325 // float percent_travel_check = (pot_check_inches / 14.5) * 100;
mdavis30 0:645ad86abcba 326 // PC.printf("B_E_POS: %d\n", pot_check); //wait(0.1); //Linear Actuator position
mdavis30 0:645ad86abcba 327 //}
mdavis30 0:645ad86abcba 328
mdavis30 0:645ad86abcba 329 //
mdavis30 0:645ad86abcba 330 //char ioc_char; //linear actuator i/o configuration
mdavis30 1:ca2454ef80d9 331 //int ioc_array[28]; //http://stackoverflow.com/questions/439573/how-to-convert-a-single-char-into-an-int
mdavis30 1:ca2454ef80d9 332
tnhnrl 2:627fd46c2b99 333 string Battery_Linear_Actuator::get_vel()
tnhnrl 2:627fd46c2b99 334 {
tnhnrl 2:627fd46c2b99 335 string vel_string = "";
tnhnrl 2:627fd46c2b99 336
tnhnrl 2:627fd46c2b99 337 MC.printf("gv\r");
tnhnrl 2:627fd46c2b99 338 wait(0.1); //THIS SHOULD WORK!
tnhnrl 2:627fd46c2b99 339
tnhnrl 2:627fd46c2b99 340 vel_string = Battery_Linear_Actuator::MC_readable_redux();
tnhnrl 2:627fd46c2b99 341 return vel_string;
tnhnrl 2:627fd46c2b99 342 }
tnhnrl 2:627fd46c2b99 343
mdavis30 1:ca2454ef80d9 344 string Battery_Linear_Actuator::get_pos()
mdavis30 1:ca2454ef80d9 345 {
mdavis30 1:ca2454ef80d9 346 string pos_string = "";
mdavis30 1:ca2454ef80d9 347
mdavis30 1:ca2454ef80d9 348 MC.printf("pos\r");
mdavis30 1:ca2454ef80d9 349 //wait(0.1); //THIS SHOULD WORK!
mdavis30 1:ca2454ef80d9 350
mdavis30 1:ca2454ef80d9 351 if(MC.readable()) //if you can read the motor controller do this...
mdavis30 1:ca2454ef80d9 352 {
mdavis30 1:ca2454ef80d9 353 while(MC.readable())
mdavis30 1:ca2454ef80d9 354 {
mdavis30 1:ca2454ef80d9 355 pos_string += MC.getc();
mdavis30 1:ca2454ef80d9 356 wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing)
mdavis30 1:ca2454ef80d9 357 }
mdavis30 1:ca2454ef80d9 358
mdavis30 1:ca2454ef80d9 359 //MAKE THIS CLEANER 6/22/17 (FIX)
mdavis30 1:ca2454ef80d9 360 //https://stackoverflow.com/questions/13294067/how-to-convert-string-to-char-array-in-c
mdavis30 1:ca2454ef80d9 361 char pos_char[1024];
mdavis30 1:ca2454ef80d9 362 strcpy(pos_char, pos_string.c_str()); //copy it to the array?
mdavis30 1:ca2454ef80d9 363
mdavis30 1:ca2454ef80d9 364 actual_position = atoi(pos_char); //convert char array to int (global variable)
mdavis30 1:ca2454ef80d9 365
mdavis30 1:ca2454ef80d9 366 return pos_string; //return the character array as a string
mdavis30 1:ca2454ef80d9 367 }
mdavis30 1:ca2454ef80d9 368
mdavis30 1:ca2454ef80d9 369 else
mdavis30 1:ca2454ef80d9 370 return "-999999"; //not working
mdavis30 1:ca2454ef80d9 371 }
mdavis30 1:ca2454ef80d9 372
mdavis30 1:ca2454ef80d9 373 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 374 {
mdavis30 1:ca2454ef80d9 375 lastTime = curr_time; //rename this later, old code
mdavis30 1:ca2454ef80d9 376 curr_time = systemTime().read();
mdavis30 1:ca2454ef80d9 377 dt = curr_time-lastTime;
mdavis30 1:ca2454ef80d9 378
mdavis30 1:ca2454ef80d9 379 //http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
mdavis30 1:ca2454ef80d9 380
mdavis30 1:ca2454ef80d9 381 //COMPUTE ERROR
mdavis30 1:ca2454ef80d9 382 la_error = la_setPoint - IMU_pitch_angle;
mdavis30 1:ca2454ef80d9 383 la_error_sum = la_error_sum + (la_error * dt); //INTEGRAL
mdavis30 1:ca2454ef80d9 384 la_derivative_error = (la_error - lastErr) / dt;
mdavis30 1:ca2454ef80d9 385
mdavis30 1:ca2454ef80d9 386 //COMPUTE PID OUTPUT
mdavis30 1:ca2454ef80d9 387 la_output = la_P_gain*la_error + la_I_gain*la_error_sum + la_D_gain*la_derivative_error;
mdavis30 1:ca2454ef80d9 388 //PC.printf("DEBUG: Linear Actuator Output: %3.3f\n", la_output);
mdavis30 1:ca2454ef80d9 389
mdavis30 1:ca2454ef80d9 390 //RECORD LAST ITERATION OF VARIABLES
mdavis30 1:ca2454ef80d9 391 lastErr = la_error;
mdavis30 1:ca2454ef80d9 392 lastTime = curr_time;
mdavis30 1:ca2454ef80d9 393
mdavis30 1:ca2454ef80d9 394 //DEBUG
mdavis30 1:ca2454ef80d9 395 //PC.printf("DEBUG: IMU PITCH ANGLE: %3.3f\n", IMU_pitch_angle);
mdavis30 1:ca2454ef80d9 396 //PC.printf("DEBUG: lastTime %3.3f, curr_time %3.3f, dt %3.3f\n", lastTime, curr_time, dt);
mdavis30 1:ca2454ef80d9 397 // 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 398 // 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 399 // 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 400
mdavis30 1:ca2454ef80d9 401 char char_buffer [256];
mdavis30 1:ca2454ef80d9 402 string Battery_Position_String;
mdavis30 1:ca2454ef80d9 403 // //DEBUG
mdavis30 1:ca2454ef80d9 404
mdavis30 1:ca2454ef80d9 405 /******** Integer wind-up prevention based on Trent's code ***********/
mdavis30 1:ca2454ef80d9 406 if (la_output > 100) //change to 500 when working properly (500 rev/min)
mdavis30 1:ca2454ef80d9 407 {
mdavis30 1:ca2454ef80d9 408 la_output = 100.0; //limit the output to +100
mdavis30 1:ca2454ef80d9 409 la_error_sum = la_error_sum - (la_error * dt);
mdavis30 1:ca2454ef80d9 410 //implement saturation instead of reset
mdavis30 1:ca2454ef80d9 411 }
mdavis30 1:ca2454ef80d9 412 else if (la_output < -100)
mdavis30 1:ca2454ef80d9 413 {
mdavis30 1:ca2454ef80d9 414 la_output = -100.0; //limit the output to -100
mdavis30 1:ca2454ef80d9 415 la_error_sum = la_error_sum + (la_error * dt);
mdavis30 1:ca2454ef80d9 416 //implement saturation instead of reset
mdavis30 1:ca2454ef80d9 417 }
mdavis30 1:ca2454ef80d9 418 /******** Integer wind-up prevention based on Trent's code ***********/
mdavis30 1:ca2454ef80d9 419
mdavis30 1:ca2454ef80d9 420 //deadband in degrees, if error is less than X deg or greater than -X deg
mdavis30 1:ca2454ef80d9 421 if (la_error < 4 && la_error > -4)
mdavis30 1:ca2454ef80d9 422 {
mdavis30 1:ca2454ef80d9 423 la_output = 0.0;
mdavis30 1:ca2454ef80d9 424 }
mdavis30 1:ca2454ef80d9 425
mdavis30 1:ca2454ef80d9 426 //send command to linear actuator motor controller
mdavis30 1:ca2454ef80d9 427 //TROY: flip the output to get it to move in the right direction...?
mdavis30 1:ca2454ef80d9 428 la_output = -la_output;
mdavis30 1:ca2454ef80d9 429 //TROY: This seems to work well. 6/19/17
mdavis30 1:ca2454ef80d9 430
mdavis30 1:ca2454ef80d9 431 int integer_output = int(la_output); //output converted to integer for the controller, only uses integer inputs in rev/min
tnhnrl 2:627fd46c2b99 432 string velocity_only_string = Battery_Linear_Actuator::velocity_only(integer_output);
mdavis30 1:ca2454ef80d9 433
mdavis30 1:ca2454ef80d9 434 //DEBUG
tnhnrl 2:627fd46c2b99 435 //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);
tnhnrl 2:627fd46c2b99 436 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 437 return char_buffer;
mdavis30 1:ca2454ef80d9 438 }
mdavis30 1:ca2454ef80d9 439 //putting the functionality into the class