File content as of revision 2:627fd46c2b99:

#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 = 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;  //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 <= 9800) //arbitrary stop at 10000
        linear_actuator_step_size += 200;
    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 >= 200)
        linear_actuator_step_size -= 200;
    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;
}

//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 = "";
    
    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");
    //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
    string velocity_only_string = 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); 
    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