File content as of revision 4:346911ee5450:

#include "Battery_Linear_Actuator.h"

#include "StaticDefs.hpp"

#include "MODSERIAL.h"
Serial MC(p28,p27);         //tx, rx (pins verified)

//CONSTRUCTOR
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 = 10000;   //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;  //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
int Battery_Linear_Actuator::MC_readable()
{
    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 = "";
    wait(0.1);        //THIS WORKS (based on testing)!
    
    if(MC.readable())   //if you can read the LA motor controller do this...
    {        
        while(MC.readable())
        {            
            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
    }
    else
    {
        return "";   //return the empty string because you didn't read anything
    }
}

string Battery_Linear_Actuator::Keyboard_O()
{      
    //PC.printf("MBED_m_a_p: %d \n", motor_absolute_position); //linear actuator position
    
    MC.printf("pos\r"); //get position from motor controller...maybe rewrite this
                        //get this from the Faulhaber Motor Controller in MATLAB?
    
    char char_buffer [60];
    string Battery_Position_String;
    sprintf(char_buffer,"MBED_m_a_p: %d \n", motor_absolute_position);      //linear actuator position
    Battery_Position_String = char_buffer;
    return Battery_Position_String;
}

string Battery_Linear_Actuator::Keyboard_H()    //Home linear actuator
{
    //reset position in program:
    motor_absolute_position = 0;
    
    MC.printf("hosp-100\r"); wait(0.1); //homing speed -100 (verified 
    MC.printf("ghosp\r");   wait(0.1);//check homing speed
    MC.printf("SHL1\r");    wait(0.1);//setup fault and AnIn
    MC.printf("gohoseq\r"); wait(0.1);//enable the sequence
    
    char homing_string[80];
    sprintf(homing_string,"\nH received! (Homing Sequence In Progress)\nMBED_m_a_p: %d \n", motor_absolute_position);                
    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)
    {
        //send velocity command and get the return value
        MC.printf("en\r");
        MC.printf("V%d\r", velocity);    
        MC.printf("GV\r", velocity);
        string velocity_return_string = Battery_Linear_Actuator::MC_readable_redux();
    
        char velocity_string[80];       
        sprintf(velocity_string, "\nVELOCITY COMMAND: V%d\nRETURN VELOCITY gv%s\n", velocity, velocity_return_string.c_str());
        
        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");
    return "\nE received!\nMOTOR ENABLED!";
}

string Battery_Linear_Actuator::Keyboard_Q() //disable linear actuator
{
    MC.printf("di\r");  
    return "\nQ received!\nMOTOR DISABLED!";
} 

string Battery_Linear_Actuator::Keyboard_EQUAL_KEY()    //INCREASE L.A. step size
{
    if (linear_actuator_step_size <= 95000) //arbitrary stop at 10000
        linear_actuator_step_size += 500;
    char bla_string[80];
    sprintf(bla_string, "\n(MBED) Linear Actuator step size INCREASED\nLAM_SZ: %d", linear_actuator_step_size);
    
    return bla_string;
}

string Battery_Linear_Actuator::Keyboard_DASH_KEY()    //DECREASE L.A. step size
{
    if (linear_actuator_step_size >= 500)
        linear_actuator_step_size -= 500;
    char bla_string[80];
    sprintf(bla_string, "\n(MBED) Linear Actuator step size DECREASED\nLAM_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 >= 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);
    
    char str[80];
    sprintf(str,"(MBED) Linear Actuator Motor Speed DECREASED\nLA_SP: %d", linear_actuator_motor_speed);
    
    return str;
}

string Battery_Linear_Actuator::Keyboard_RIGHT_BRACKET() //disable linear actuator
{                                                   //100 to 30000, INCREASE (100 increments)
    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);
    
    char str[80];
    sprintf(str,"(MBED) Linear Actuator Motor Speed INCREASED\nLA_SP: %d", linear_actuator_motor_speed);
    
    return str;
}

//http://stackoverflow.com/questions/8011700/how-do-i-extract-specific-n-bits-of-a-32-bit-unsigned-integer-in-c
unsigned createMask(unsigned a, unsigned b) {
   unsigned r = 0;
   for (unsigned i=a; i<=b; i++)
       r |= 1 << i;

   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
        map_check = motor_absolute_position - linear_actuator_step_size;
        if (map_check >= 0) 
            motor_absolute_position -= linear_actuator_step_size;
    }
                    
    MC.printf("la%d\r", motor_absolute_position);
    MC.printf("m\r");
    
    char position[80];
    //sprintf(position, "\nA received! (MBED) motor_absolute_position\nMBED_m_a_p: %d\n", motor_absolute_position);
    sprintf(position, "\nA received! (MBED) motor_absolute_position\nLAM_POS: %d\n", motor_absolute_position);
    return position;
}

//EXTEND
string Battery_Linear_Actuator::Keyboard_D()
{
    if (motor_absolute_position <= 175000)
    {
        //explicitly written, check if this will not go past 175000
        map_check = motor_absolute_position + linear_actuator_step_size;
        if (map_check <= 175000) 
            motor_absolute_position += linear_actuator_step_size;
    }
                    
    MC.printf("la%d\r", motor_absolute_position);
    MC.printf("m\r");
    
    char position[80];
    //sprintf(position, "\nA received! (MBED) motor_absolute_position\nMBED_m_a_p: %d\n", motor_absolute_position);
    sprintf(position, "\nA received! (MBED) motor_absolute_position\nLAM_POS: %d\n", motor_absolute_position);
    return position;
}

string Battery_Linear_Actuator::get_vel()
{
    string vel_string = "";
    
    MC.printf("gv\r");
    wait(0.1);        //THIS SHOULD WORK!
    
    vel_string = Battery_Linear_Actuator::MC_readable_redux();
    return vel_string;
}

string Battery_Linear_Actuator::get_pos()
{
    string pos_string = "";
    
    MC.printf("pos\r");
    
    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)
{
    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;

    char char_buffer [256];
    string Battery_Position_String;
            
    /******** 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;
    
    int integer_output = int(la_output); //output converted to integer for the controller, only uses integer inputs in rev/min
    
    //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?";
    
    //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