Diff: Battery_Linear_Actuator.cpp

Revision:

1:ca2454ef80d9

Parent:

0:645ad86abcba

Child:

2:627fd46c2b99

--- a/Battery_Linear_Actuator.cpp	Fri Jun 09 17:32:14 2017 +0000
+++ b/Battery_Linear_Actuator.cpp	Fri Jun 30 18:32:40 2017 +0000
@@ -1,5 +1,7 @@
 #include "Battery_Linear_Actuator.h"
 
+#include "StaticDefs.hpp"
+
 #include "MODSERIAL.h"
 Serial MC(p28,p27);         //tx, rx (pins verified)
 
@@ -7,12 +9,32 @@
 Battery_Linear_Actuator::Battery_Linear_Actuator()
 {
     MC.baud(9600);  //motor controller to MBED connection speed
+    
+    MC.printf("sp100"); //set the speed to 100, test this until I can safely set to 500
+    
     motor_absolute_position = 0;            //initialize variables here
     
     linear_actuator_step_size = 1000;   //default confirmed with Dan Edwards
     linear_actuator_motor_speed = 500;  //5 seconds of full travel between 0 and 175,000 (1000 is about 3 seconds)
     
-    map_check = 0;
+    map_check = 0;  //remove?
+    
+    
+//    la_P_gain = 1.0;  //was 0.10
+//    la_I_gain = 0.00;
+//    la_D_gain = 0.00;
+    curr_time = 0.00;
+    dt = 0.00;    
+    la_error = 0.00;
+    la_error_sum = 0.00;  
+    la_derivative_error = 0.00;  
+    lastErr = 0.00;   
+    lastTime = 0.00;  
+    
+    la_integral = 0.00;
+    la_output = 0.00;
+    
+    actual_position = 0;
 }
 
 //KEYBOARD FUNCTIONS
@@ -21,6 +43,34 @@
     return MC.readable();
 }
 
+char Battery_Linear_Actuator::MC_getc()
+{
+    return MC.getc();   //return this character to whatever is calling the function, useful for getting Motor Controller data
+}
+
+string Battery_Linear_Actuator::MC_readable_redux()
+{
+    string bla_string = "";
+    
+    //MC.printf("pos\r");
+    wait(0.1);        //THIS SHOULD WORK!
+    
+    if(MC.readable())   //if you can read the motor controller do this...
+    {
+        //bla_string += "\nMC readable\n";
+        
+        while(MC.readable())
+        {
+            //strcat(bla_array, MC.getc()); //this is a pass-through of the MC (getc) to the PC (putc)
+            
+            bla_string += MC.getc();
+            wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing)
+        }
+    }
+    
+    return bla_string;   //return the character array as a string
+}
+
 string Battery_Linear_Actuator::Keyboard_O()
 {      
     //PC.printf("MBED_m_a_p: %d \n", motor_absolute_position); //linear actuator position
@@ -50,6 +100,88 @@
     return homing_string;
 }
 
+string Battery_Linear_Actuator::Keyboard_J() //L.A. POSITION ONLY
+{
+    MC.printf("pos\r");
+    return "\nL.A. POSITION COMMAND RECEIVED.\n";
+}
+
+string Battery_Linear_Actuator::Velocity(int velocity) //EXTEND with velocity command
+{
+    string la_position = Battery_Linear_Actuator::get_pos();  //get the position data in  string format
+
+    //https://stackoverflow.com/questions/13294067/how-to-convert-string-to-char-array-in-c
+    char la_position_char[1024];
+    strcpy(la_position_char, la_position.c_str());  //copy it to the array?
+
+    //back to the drawing board
+    //https://developer.mbed.org/questions/2910/Convert-string-to-int/
+
+    int int_position = atoi(la_position_char);
+
+    if (int_position >= 0 && int_position <= 175000)
+    {
+        MC.printf("en\r");
+        MC.printf("V%d\r", velocity);    
+    
+        char velocity_string[80];
+        sprintf(velocity_string, "\nVELOCITY COMMAND: V%d.\n", velocity);
+    
+        return velocity_string;
+    }
+    
+    else
+        return "outside of bounds";
+}
+
+string Battery_Linear_Actuator::velocity_only(int velocity) //EXTEND with velocity command
+{
+    //this uses the position record saved in the object
+    if (actual_position >= 0 && actual_position <= 175000)
+    {
+        MC.printf("en\r");
+        MC.printf("V%d\r", velocity);    
+    
+        char velocity_string[80];
+        sprintf(velocity_string, "\nVELOCITY COMMAND: V%d.\n", velocity);
+    
+        return velocity_string;
+    }
+    
+    else
+        return "outside of bounds"; //maybe change this
+}
+
+string Battery_Linear_Actuator::Keyboard_K() //EXTEND with velocity command
+{
+    if (motor_absolute_position >= 0 && motor_absolute_position <= 175000)
+    {   
+        MC.printf("en\r");
+        MC.printf("V50\r");
+        return "\nVELOCITY COMMAND. Extending.\n";
+    }
+    else
+        return "\nmotor position out of bounds (below 0 or above 175000)\n";
+}
+
+string Battery_Linear_Actuator::Keyboard_L() //RETRACT with velocity command
+{
+    if (motor_absolute_position >= 0 && motor_absolute_position <= 175000)
+    {   
+        MC.printf("en\r");
+        MC.printf("V-50\r");
+        return "\nVELOCITY COMMAND. Retracting.\n";
+    }
+    else
+        return "\nmotor position out of bounds (below 0 or above 175000)\n";
+}
+string Battery_Linear_Actuator::Keyboard_U()
+{
+    MC.printf("v0\r");  //velocity 0
+    MC.printf("di\r");  //motor disabled
+    return "\nVelocity=0, L.A. motor disabled.\n";
+}
+
 string Battery_Linear_Actuator::Keyboard_E() //enable linear actuator
 {
     MC.printf("en\r");
@@ -67,7 +199,7 @@
     if (linear_actuator_step_size <= 9800) //arbitrary stop at 10000
         linear_actuator_step_size += 200;
     char bla_string[80];
-    sprintf(bla_string, "\n(MBED) Linear Actuator step size INCREASED: %d\nLA_SZ: %d", linear_actuator_step_size, linear_actuator_step_size);
+    sprintf(bla_string, "\n(MBED) Linear Actuator step size INCREASED\nLA_SZ: %d", linear_actuator_step_size);
     
     return bla_string;
 }
@@ -77,15 +209,15 @@
     if (linear_actuator_step_size >= 200)
         linear_actuator_step_size -= 200;
     char bla_string[80];
-    sprintf(bla_string, "\n(MBED) Linear Actuator step size DECREASED\nLA_SZ: %d", linear_actuator_step_size, linear_actuator_step_size);
+    sprintf(bla_string, "\n(MBED) Linear Actuator step size DECREASED\nLA_SZ: %d", linear_actuator_step_size);
     
     return bla_string;
 }
 
 string Battery_Linear_Actuator::Keyboard_LEFT_BRACKET()
 {                                                   //100 to 30000, DECREASE (100 increments)
-    if (linear_actuator_motor_speed >= 100)
-        linear_actuator_motor_speed -= 100;
+    if (linear_actuator_motor_speed >= 200)
+        linear_actuator_motor_speed -= 200;
     //PC.printf("\n(MBED) Linear Actuator Motor Speed INCREASED: sp%d\n", linear_actuator_motor_speed);
     MC.printf("sp%d\r", linear_actuator_motor_speed); //retesting linear actuator motor speed 5/11/17
     //PC.printf("\nLAMSP: %d\n", linear_actuator_motor_speed);
@@ -98,8 +230,8 @@
 
 string Battery_Linear_Actuator::Keyboard_RIGHT_BRACKET() //disable linear actuator
 {                                                   //100 to 30000, INCREASE (100 increments)
-    if (linear_actuator_motor_speed <= 29900)
-        linear_actuator_motor_speed += 100;
+    if (linear_actuator_motor_speed <= 29800)
+        linear_actuator_motor_speed += 200;
     //PC.printf("\n(MBED) Linear Actuator Motor Speed INCREASED: sp%d\n", linear_actuator_motor_speed);
     MC.printf("sp%d\r", linear_actuator_motor_speed); //retesting linear actuator motor speed 5/11/17
     //PC.printf("\nLAMSP: %d\n", linear_actuator_motor_speed);
@@ -119,8 +251,14 @@
    return r;
 }
 
+//RETRACT
 string Battery_Linear_Actuator::Keyboard_A()
 {
+//    float time_read = systemTime().read();                  //added #include "StaticDefs.hpp" on 6/14/17
+//    char position[80];
+//    sprintf(position, "\nSYSTEM TIME IS %f\n", time_read);  //SYSTEM TIME IS 24.918480
+//    return position;
+    
     if (motor_absolute_position >= 0)
     {
         //explicitly written, check if this will not go past 175000
@@ -137,6 +275,7 @@
     return position;
 }
 
+//EXTEND
 string Battery_Linear_Actuator::Keyboard_D()
 {
     if (motor_absolute_position <= 175000)
@@ -186,4 +325,100 @@
 
 //
 //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
\ No newline at end of file
+//int ioc_array[28];      //http://stackoverflow.com/questions/439573/how-to-convert-a-single-char-into-an-int
+
+string Battery_Linear_Actuator::get_pos()
+{
+    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
+}
+
+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)
+{
+    lastTime = curr_time;                      //rename this later, old code
+    curr_time = systemTime().read();
+    dt = curr_time-lastTime;
+
+    //http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
+
+    //COMPUTE ERROR
+    la_error = la_setPoint - IMU_pitch_angle;
+    la_error_sum = la_error_sum + (la_error * dt);  //INTEGRAL
+    la_derivative_error = (la_error - lastErr) / dt;
+            
+    //COMPUTE PID OUTPUT
+    la_output = la_P_gain*la_error + la_I_gain*la_error_sum + la_D_gain*la_derivative_error;
+    //PC.printf("DEBUG: Linear Actuator Output: %3.3f\n", la_output);
+        
+    //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)
+    {    
+        la_output = 100.0;    //limit the output to +100
+        la_error_sum = la_error_sum - (la_error * dt);
+        //implement saturation instead of reset
+    }
+    else if (la_output < -100)  
+    {
+        la_output = -100.0;   //limit the output to -100
+        la_error_sum = la_error_sum + (la_error * dt);
+        //implement saturation instead of reset
+    }
+    /******** Integer wind-up prevention based on Trent's code ***********/
+    
+    //deadband in degrees, if error is less than X deg or greater than -X deg
+    if (la_error < 4 && la_error > -4)
+    {
+        la_output = 0.0;
+    }
+    
+    //send command to linear actuator motor controller
+    //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
+    Battery_Linear_Actuator::velocity_only(integer_output);
+    
+    //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); 
+    return char_buffer;
+}
+//putting the functionality into the class
\ No newline at end of file