Diff: Battery_Linear_Actuator.cpp

Revision:

3:9181d42863cd

Parent:

2:627fd46c2b99

--- a/Battery_Linear_Actuator.cpp	Mon Jul 17 14:23:34 2017 +0000
+++ b/Battery_Linear_Actuator.cpp	Fri Jul 21 13:29:09 2017 +0000
@@ -297,39 +297,6 @@
     return position;
 }
 
-//void Keyboard_P()
-//{
-//    //Buoyancy Engine gets about 14.5 inches of travel before it hits end of screw (inner piston)
-//    //max voltage on potentiometer is 3.06 volts
-//    //min voltage on potentiometer is 0.70 volts
-//    PC.printf("\nP received!\n");
-//    float pot_check = potentiometer_analog_in.read() * 3.3;
-//    float pot_check_inches = 6.144 * pot_check - 4.3;
-//    float percent_travel_check = (pot_check_inches / 14.5) * 100;               
-//    //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].
-////    PC.printf("Potentiometer Reading: %f volts %f inches %f%c\n ", pot_check, pot_check_inches, percent_travel_check, 37); 
-////    PC.printf("\n(MBED) Linear Actuator position: %d\n", motor_absolute_position); //wait(0.1); //Linear Actuator position
-//    
-//    PC.printf("B_E_POS: %d\n", pot_check); //wait(0.1); //Linear Actuator position
-//    PC.printf("MBED_m_a_p: %d \n", motor_absolute_position);
-//    
-//    MC.printf("pos\r");
-//    //MC.printf("pos\r"); wait(0.1); //position
-//    //get the next line from the Faulhaber Motor Controller in MATLAB?
-//}
-
-//void BE_POSITION()
-//{
-//    float pot_check = potentiometer_analog_in.read() * 3.3;
-//    float pot_check_inches = 6.144 * pot_check - 4.3;
-//    float percent_travel_check = (pot_check_inches / 14.5) * 100;               
-//    PC.printf("B_E_POS: %d\n", pot_check); //wait(0.1); //Linear Actuator position
-//}
-
-//
-//char ioc_char;          //linear actuator i/o configuration
-//int ioc_array[28];      //http://stackoverflow.com/questions/439573/how-to-convert-a-single-char-into-an-int
-
 string Battery_Linear_Actuator::get_vel()
 {
     string vel_string = "";
@@ -346,28 +313,9 @@
     string pos_string = "";
     
     MC.printf("pos\r");
-    //wait(0.1);        //THIS SHOULD WORK!
     
-    if(MC.readable())   //if you can read the motor controller do this...
-    {       
-        while(MC.readable())
-        {
-            pos_string += MC.getc();
-            wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing)
-        }
-        
-        //MAKE THIS CLEANER 6/22/17 (FIX)
-        //https://stackoverflow.com/questions/13294067/how-to-convert-string-to-char-array-in-c
-        char pos_char[1024];
-        strcpy(pos_char, pos_string.c_str());  //copy it to the array?
-
-        actual_position = atoi(pos_char);  //convert char array to int (global variable)
-        
-        return pos_string;   //return the character array as a string
-    }
-    
-    else
-        return "-999999"; //not working
+    pos_string = Battery_Linear_Actuator::MC_readable_redux();
+    return pos_string;
 }
 
 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)
@@ -390,17 +338,9 @@
     //RECORD LAST ITERATION OF VARIABLES
     lastErr = la_error;
     lastTime = curr_time;
-    
-    //DEBUG
-    //PC.printf("DEBUG: IMU PITCH ANGLE: %3.3f\n", IMU_pitch_angle);
-    //PC.printf("DEBUG: lastTime %3.3f, curr_time %3.3f, dt %3.3f\n", lastTime, curr_time, dt);
-//    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);    
-//    PC.printf("> DEBUG: la_output: %3.3f IMU PITCH ANGLE: %3.3f SETPOINT: %3.3f\n", la_output, IMU_pitch_angle, la_setPoint);
-//    PC.printf("DEBUG: P:%3.3f I:%3.3f D:%3.3f\n", la_P_gain, la_I_gain, la_D_gain);
 
     char char_buffer [256];
     string Battery_Position_String;
-//  //DEBUG
             
     /******** Integer wind-up prevention based on Trent's code ***********/
     if (la_output > 100)   //change to 500 when working properly (500 rev/min)
@@ -424,16 +364,34 @@
     }
     
     //send command to linear actuator motor controller
-    //TROY: flip the output to get it to move in the right direction...?
+    //TROY: flip the output to get it to move in the right direction...
     la_output = -la_output;
-    //TROY: This seems to work well.  6/19/17
     
     int integer_output = int(la_output); //output converted to integer for the controller, only uses integer inputs in rev/min
-    string velocity_only_string = Battery_Linear_Actuator::velocity_only(integer_output);
+    
+    //get linear actuator position
+    const char *char_actual_position = Battery_Linear_Actuator::get_pos().c_str();
+    //actual_position_string = BLA_object.get_pos().c_str();
+
+    double double_actual_position = 0.0;                
+    sscanf(char_actual_position, "%lf", &double_actual_position);   //convert to double
+     
+    //IF THE POSITION IS BETWEEN 0 and 175,000 only, then send the velocity
+    string velocity_only_string = "velocity not sent?";
     
-    //DEBUG
-    //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); 
-    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); 
+    //found this out through testing
+    //if velocity is positive you extend the nut
+    //but you don't want it to extend past the limit of 175,000
+    if ((integer_output > 0) && (double_actual_position <= 180000.0))
+        string velocity_only_string = Battery_Linear_Actuator::velocity_only(integer_output);   //send the velocity command if this condition is true
+    else if ((integer_output < 0) && (double_actual_position >= 3000.0)) //it's a little jumpy (so go here instead of zero)
+        string velocity_only_string = Battery_Linear_Actuator::velocity_only(integer_output);   //send the velocity command if this condition is true
+    else //conditions where we go past the limit
+        string velocity_only_string = Battery_Linear_Actuator::velocity_only(0);                //send the velocity command if this condition is true
+        
+    //use this printout to debug it
+    sprintf(char_buffer, "What is the position from the PID loop: %lf\n", double_actual_position); 
+    //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); 
     return char_buffer;
 }
 //putting the functionality into the class
\ No newline at end of file