File content as of revision 5:7421776f6b08:

#include "mbed.h"
#include "MODSERIAL.h" //for IMU (and got rid of regular serial library) #include "IMU_code.h" 
#include "StaticDefs.hpp"
#include "ltc1298.hpp"

#include <cstdlib>

#include <string>
using namespace std;

bool debug_mode = false;

#include "IMU_code.h"                   //IMU code

#include "Battery_Linear_Actuator.h"    //Battery Linear Actuator code (TROY)         (FIX INCLUSION ISSUES, ports)
 
Serial PC(USBTX,USBRX);     //tx, rx

extern "C" void mbed_reset();   //utilized to reset the mbed through the serial terminal

char Key;

string IMU_STRING = "";


DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

AnalogIn   pressure_analog_in(A5); //Initialize pin20   (read is float value)
AnalogIn   ain(p18);

/* ************ These tickers work independent of any while loops ********** */
Ticker IMU_ticker; //ticker for printing IMU //https://developer.mbed.org/handbook/Ticker
Ticker BE_position_ticker;  //probably delete soon
Ticker PRESSURE_ticker;

Ticker BCE_ticker;  //new 6/5/17
Ticker PID_ticker;  //new 6/14/17
Ticker LA_ticker;   //new 6/22/17

float positionCmd = 200.0;  //250

/* ************************************************************************* */ 

float pi = 3.14159265359;

/* PID LOOP STUFF */
float la_setPoint = 0.00;       //the IMU pitch angle we want (setpoint)

float la_P_gain = 1.0;
float la_I_gain = 0.00;
float la_D_gain = 0.00;
/* PID LOOP STUFF */

float IMU_pitch_angle = 0.00;

bool motor_retracting = false;
bool motor_extending = false;

// 7/10/17
string actual_position_string = "";
double double_actual_position = 0.00;

void IMU_ticking()
{
    led1 = !led1;   //flash the IMU LED
    
    if (debug_mode)
        PC.printf("%s\n", IMU_STRING.c_str());  //if there's something there, print it
}

void PRESSURE_ticking()
{
    if (debug_mode)
        PC.printf("ressure: %f psi \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845);    //read the analog pin
    //this voltage has been checked and scaled properly (6/28/2017)
}

void BCE_ticking()  //new 6/5/17
{
    if (debug_mode)
        PC.printf("Buoyancy_Engine_POS: %3.0f mm  BE_vel: %2.2f mm/s  Set Point %3.0f  posCon.getOutput: % 1.3f \n", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput());
    
    //PC.printf("BE_pos: %3.0f mm  BE_vel: %2.2f mm/s  Set Point %3.0f  controller output: % 1.3f \n", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput());
}

int main()
{  
    PC.baud(9600);  //mbed to PC serial connection speed
    //PC.baud(230400);
    //got screwy when i changed it
    hBridge().stop();
    
    PC.printf("* * * * * * * * * * * * * * * * *\n");
    PC.printf("PV TEST PROGRAM STARTED 7/13/2017\n");
    PC.printf("* * * * * * * * * * * * * * * * *\n");
    
    systemTime().start();   //start the timer, needed for PID loop
        
    /* **************** Linear Actuator MOTOR CONTROLLER **************** */
    Battery_Linear_Actuator BLA_object;             //create the IMU object from the imported class  
    
    PC.printf("%s\n", BLA_object.Keyboard_U().c_str());     //velocity = 0, motor disabled
    
    PC.printf("%s\n", BLA_object.Keyboard_Q().c_str());     //turn off motor
    wait(1);
    PC.printf("%s\n", BLA_object.Keyboard_E().c_str());     //turn on motor
    wait(1);
    
    //setup and start the adc. This runs on a fixed interval and is interrupt driven
    adc().initialize();
    adc().start();

    //Initialize the position velocity filter. This will consume a couple of seconds for
    //the filter to converge
    pvf().init();

////CHANGED TO GLOBAL VARIABLES
    float motor_cmd = 0.0;
//   float positionCmd = 250.0;
    float P = 0.10;
    float I = 0.00;
    float D = 0.00;
    float count = 0.0;
    //char userInput;   //from Trent's code?

    float la_step = 1.0;
    float la_setPoint_temp = 0.0;
    
    //Start off in manual mode for testing.
    bool BCE_auto = false;
    bool LA_auto = false;

    float bce_auto_step_raw = 1.0;
    float bce_auto_step_l;
    //float convert = 10000;
    float convert = 1;
    float bce_auto_step_ml = bce_auto_step_raw * convert;
    int bce_manual_step = 10;
    //float volume_bce = 90.0*convert;  //Troy: Not sure I get the conversion
    float volume_bce = 0;
    float positionCmd_temp;
    //float ml_to_l= 0.000000001; //Is this a milliliter? TROY: 7/10/17
    float ml_to_l= 0.001; //milliliter???? TROY: 7/10/17

    hBridge().run(motor_cmd);
    
    //set the intial gains for the position controller
    posCon().setPgain(P);
    posCon().setIgain(I);
    posCon().setDgain(D);
    posCon().writeSetPoint(positionCmd);    //7/13/17 initialize the position of the motor to 200
    
    PC.printf("BCE Test Program Started!\n");
    wait(1);
    
    /* *************************** LED *************************** */
    led1 = 1;   //initial values
    led2 = 1;
    led3 = 0;
    led4 = 1;    
    /* *************************** LED *************************** */

    int la_cases = 0;
    int count_while = 0;
    //hBridge().reset();
    PC.printf("\n```````````Linear Actuator in IMU controlled mode````````````\n\n");
    //PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n");
    
    /* ************************** Pressure Sensor ************************** */
    PRESSURE_ticker.attach(&PRESSURE_ticking, 3.0);
    /* ************************** Pressure Sensor ************************** */
    
    /* *************************** MOTOR CONTROLLER *************************** */
    //Battery_Linear_Actuator BLA_object;               //create the IMU object from the imported class
    /* *************************** MOTOR CONTROLLER *************************** */
    
    /* *************************** IMU *************************** */
    IMU_code IMU_object;               //create the IMU object from the imported class
    IMU_ticker.attach(&IMU_ticking, 3.0);
    /* *************************** IMU *************************** */
    
    /* *************************** BCE *************************** */
    //float previous_positionCmd = -1;
    //BCE_ticker.attach(&BCE_ticking, 3.0);
    /* *************************** BCE *************************** */
    
    while(1) 
    {
        //PC.printf("DEBUG: POT position: %f velocity: %f adc_count: %d VS conv_distance: %f adc_ch0_filter: %f\n ", pvf().getPosition(), pvf().getVelocity(), adc().ch0_filt, pvf().getVelocity(), adc().get_ch0_filt); //DEBUG TROY
        if (debug_mode)
        {
            PC.printf("DEBUG: POT position: %6.3f velocity: %6.3f adc_count: %d VS conv_distance: %6.3f adc_ch0_filter: %6.3f\n", pvf().getPosition(), pvf().getVelocity(), adc().ch0_filt, pvf().get_conv_distance(), adc().get_ch0_filt()); //DEBUG TROY
            PC.printf("DEBUG: dt: %f current_time: %f last_time: %f\n", pvf().getDt(), pvf().get_curr_time(), pvf().get_last_time()); //DEBUG TROY
            PC.printf("DEBUG: x1: %f x2: %f x1_dot: %f x2_dot: %f\n", pvf().get_x1(), pvf().get_x2(), pvf().get_x1_dot(), pvf().get_x2_dot()); //DEBUG TROY
        }
        
        /* *************************** IMU *************************** */
        IMU_STRING = IMU_object.IMU_run(); //grab the IMU string each iteration through the loop
        IMU_pitch_angle = 1.0 * IMU_object.IMU_pitch(); //get the pitch update constantly?        
        
        if (debug_mode)
            PC.printf("pitch angle... %f    set pitch angle: %f\n", IMU_pitch_angle, la_setPoint); 
        /* *************************** IMU *************************** */
        
        /*          Buoyancy Engine         */
        // update the position velocity filter
        pvf().update();

        //update the controller with the current numbers in the position guesser
        posCon().update(pvf().getPosition(), pvf().getVelocity(), pvf().getDt()) ;
        hBridge().run(posCon().getOutput());

        /*          Buoyancy Engine         */
        
        //FOR DEBUGGING
        //PC.printf("BE_pos: %3.0f mm  BE_vel: %2.2f mm/s  Set Point %3.0f  controller output: % 1.3f P: %1.3f I: %1.4f D: %1.4f\r", pvf().getPosition(), pvf().getVelocity(), positionCmd, posCon().getOutput(), P, I, D);
    
        if (PC.readable())
        {
            led4 = !led4;           //read indicator changes
            Key=PC.getc();
            
        //Universal MBED Controls 
            if(Key == '!')  //RESET THE MBED
            {
                PC.printf("MBED RESET KEY (!) PRESSED\n");
                PC.printf("Linear Actuator Motor disabled!\n");
                //disable the motor
                BLA_object.Keyboard_Q();        //DISABLE THE MOTOR
                wait(0.5);                      //500 milliseconds
                mbed_reset(); //reset the mbed!
            }
            
            else if(Key == '~')     // (shift + '`')
            {
                debug_mode = !debug_mode; //shift it back and forth
                PC.printf(" # # # # # # DEBUG MODE: %d # # # # # # \n", debug_mode);
            }
            
            else if(Key == 'H')  //homing sequence
            {
                PC.printf("### homing the device ###");
                BLA_object.Keyboard_H();
                wait(5); //for debugging
                
                ////TEST THIS
                //PC.printf("BE_pos: 0\n");
                //PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str());     //flip this back and forth
                
                //TROY: TEST THIS 7/11/2017
                const char *char_actual_position = BLA_object.get_pos().c_str();
                //actual_position_string = BLA_object.get_pos().c_str();
                
                sscanf(char_actual_position, "%lf", &double_actual_position);
                // 7/10/17
                
                PC.printf("### position is\nBEP: %lf ###\n", double_actual_position); 
                
                wait(1); //for debugging       
            }
            else if(Key == 'p' or Key == 'P')
            {
                // 7/10/17
                //actual_position_string = BLA_object.get_pos();
                //actual_position_string = BLA_object.get_pos().c_str();
                //const char *char_actual_position = string_actual_position.c_str();
                const char *char_actual_position = BLA_object.get_pos().c_str();
                //actual_position_string = BLA_object.get_pos().c_str();
                
                sscanf(char_actual_position, "%lf", &double_actual_position);
                // 7/10/17
                
                PC.printf("### position is\nBEP: %lf ###\n", double_actual_position);     //flip this back and forth
                wait(1); //for debugging      
                // "-999999" means it is not working 
            }
            
        //Buoyancy Engine Controls
            else if (Key == ',' or Key == '<')
            {
                if (BCE_auto == false)
                {
                    PC.printf("BCE: Now in Automatic Mode\n");
                    BCE_auto = true;
                }
                else
                {
                    PC.printf("BCE: Still in Automatic Mode\n");
                }
            }
            else if (Key == '.' or Key == '>')
            {
                if (BCE_auto == true)
                {
                    PC.printf("BCE: Now in Manual Mode\n");
                    BCE_auto = false;
                }
                else
                {
                    PC.printf("BCE: Still in Manual Mode\n");
                }
            }
        //BCE Automatic Controls
            else if (Key == 's' or Key == 'S')
            {
                if (BCE_auto == true)
                {
                    //PC.printf("BCE Automatic Step Size Change\n");
                    if (bce_auto_step_raw == 1.0)
                    {
                        bce_auto_step_raw = 5.0;
                    }
                    else if (bce_auto_step_raw == 5.0)
                    {
                        bce_auto_step_raw = 10.0;
                    }
                    else if (bce_auto_step_raw == 10.0)
                    {
                        bce_auto_step_raw = 50.0;
                    }
                    else if (bce_auto_step_raw == 50.0)
                    {
                        bce_auto_step_raw = 100.0;
                    }
                    else if (bce_auto_step_raw == 100.0)
                    {
                        bce_auto_step_raw = 1.0;
                    }
                    bce_auto_step_ml = bce_auto_step_raw * convert;
                    PC.printf("BCE Auto Step Size Now\n BE_ST_ML: %7.0f milliliters\n", bce_auto_step_ml); 
                }
                else
                {
                    PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
                
                }
            }
            else if(Key == 'd' or Key == 'D')       //0 mm = 0 mL, 350 mm = 1816 mL
            {
                PC.printf("(d) volume_bce: %f\n", volume_bce);
                if (BCE_auto == true)
                {
                    PC.printf("(d) BCE_auto: %d\n", BCE_auto);
                    if (volume_bce >= 1) //350 mm retracted from end = 1816 mL in buyoancy engine tube
                    {
                        volume_bce -= bce_auto_step_ml;
                        float calc_height = (volume_bce * 1000) / (pi*40.64*40.64);
                        PC.printf("Buoyancy Engine Volume VBE: %1.5f milliliters (Distance: %f mm)\n", volume_bce, calc_height); //to read in MATLAB 
                    }
                    else if (volume_bce < 1)
                    {
                        PC.printf("Volume reset to 1 mL!\n");   //keep the volume at zero mL
                        volume_bce = 1;
                    }
                }
                else
                {
                    PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
                }
            }
            else if(Key == 'f' or Key == 'F')
            {
                PC.printf("(f) volume_bce: %f\n", volume_bce);
                if (BCE_auto == true)
                {
                    PC.printf("(f) BCE_auto: %d\n", BCE_auto);
                    if (volume_bce <= 1816) //350 mm retracted from end = 1816 mL in buyoancy engine tube
                    {
                        volume_bce += bce_auto_step_ml;
                        float calc_height = (volume_bce * 1000) / (pi*40.64*40.64);
                        PC.printf("Buoyancy Engine Volume VBE: %1.5f milliliters (Distance: %f mm)\n", volume_bce, calc_height); //to read in MATLAB 
                    }
                    else if (volume_bce > 1816)
                    {
                        PC.printf("Volume reset to 1816 mL (max volume)!\n");   //keep the volume at 1816 mL (max)
                        volume_bce = 1816;
                    }
                }
                else
                {
                    PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
                }
            }
            else if(Key == 'r' or Key == 'R')
            {
                if (BCE_auto == true)
                {
                    PC.printf("\nR received!\n");
                    
                    //Troy equation (volume 1 mL = 1000 mm^3)
                    positionCmd = (volume_bce * 1000) / (pi*40.64*40.64);   //volume / (pi * r^2)
                    PC.printf("\nBCE volume sent was %d mL (distance: %f mm)\n", volume_bce, positionCmd);
                    PC.printf("(BCE Distance) VBE_SENT: %3.0f\n", positionCmd);
                    
                    posCon().writeSetPoint(positionCmd);    //write the setPoint (target)
                    
                    hBridge().run(posCon().getOutput());    //run the h-bridge until it reaches the target
                
                    hBridge().reset();                      //reset to start this process of moving the h-bridge
                
                    count = 0;                              //not sure...
                }
                else
                {
                    PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
                }
            }
        //BCE Manual Controls
           else if (Key == '2' or Key == '@')
            {
                if (BCE_auto == false)
                {
                    PC.printf("BCE Manual Step Size Change\n");
                    if (bce_manual_step == 1)
                    {
                        bce_manual_step = 10;
                    }
                    else if (bce_manual_step == 10)
                    {
                        bce_manual_step = 25;
                    }
                    else if (bce_manual_step == 25)
                    {
                        bce_manual_step = 50;
                    }
                    else if (bce_manual_step == 50)
                    {
                        bce_manual_step = 1;
                    }

                    PC.printf("BCE Manual Step Size Now\nBEM_ST: %d\n", bce_manual_step);
                }
                else
                {
                    PC.printf("ERROR: BCE in Auto Mode, this is a manual command\n");
                }
            }
            else if (Key == 'z' or Key == 'Z') 
            {
                if (BCE_auto == false)
                {
                    positionCmd -= bce_manual_step;
                    //PC.printf(">>> BEM_P: %3.0f\n", positionCmd); //to read in MATLAB (DEBUG)
                    
                    //decrement the duty cycle
                    if (positionCmd >= 50 && positionCmd <=350)     //limit buoyancy engine position 25 to 375
                    {
                        PC.printf("Commanded BCE position is BEM_P: %3.0f\n", positionCmd); //to read in MATLAB
                    }
                    else if (positionCmd < 50)
                    {
                        PC.printf("BCE past limits! Reset to 50\n"); //to read in MATLAB
                        positionCmd = 50;
                    }
                }
                else
                {
                    PC.printf("ERROR: BCE in Auto Mode, this is a manual command\n");
                }
            }
            else if (Key == 'l' or Key == 'L') 
                PC.printf("DEBUG: String position? %f velocity? %f (BCE active: %d)\n", pvf().getPosition(), pvf().getVelocity(),posCon().getOutput()); //DEBUG TROY
            
            else if (Key == 'x' or Key == 'X') 
            {
                if (BCE_auto == false)
                {
                    positionCmd += bce_manual_step;
                    //PC.printf(">>> BEM_P: %3.0f\n", positionCmd); //to read in MATLAB (DEBUG)

                    //decrement the duty cycle
                    if (positionCmd >= 50 && positionCmd <=350)     //limit buoyancy engine position 25 to 375
                    {
                        PC.printf("Commanded BCE position is BEM_P: %3.0f\n", positionCmd); //to read in MATLAB
                    }
                    else if (positionCmd >350)
                    {
                        PC.printf("BCE past limits! Reset to 350\n"); //to read in MATLAB
                        positionCmd = 350;
                    }
                }
                else
                {
                    PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
                }
            }
            else if(Key == 'w' or Key == 'W')
            {
                if (BCE_auto == false)
                {
                    PC.printf("\nW received!\n");
                    PC.printf("BEM_SND: %3.0f\n", positionCmd);
                                        
                    posCon().writeSetPoint(positionCmd);    //writing once doesn't work sometimes
                    
                    //posCon().setPgain(P);
                    //posCon().setIgain(I);
                    //posCon().setDgain(D);
                    hBridge().run(posCon().getOutput());
                
                    hBridge().reset();
                
                    count = 0;

                }
                else
                {
                    PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
                }
            }
            
        //Linear Actuator Controls
            else if(Key == 'c' or Key == 'C')
            {
                if (LA_auto == true)
                {
                    PC.printf("ERROR: LA already in auto mode\n");
                }
                else
                {
                    LA_auto = true;
                    PC.printf("```````````LA now in IMU (Auto) Controlled Mode```````````````\n");
                    la_cases = 0;
                    count_while = 0;
                }
            }
            else if (Key == 'v' or Key == 'V')
            {
                if (LA_auto == true)
                {
                    LA_auto = false;
                    //Change cases: go from imu controlled to manual
                    PC.printf("```````````LA now in Manual Mode````````````````````\n");
                    la_cases = 1;
                    count_while = 0;
                }
                else 
                {
                    PC.printf("ERROR: LA already in manual mode\n");
                    PC.printf("LA_auto ==> %d\n", LA_auto); //should show "0" (false)
                }
            }
            else if (Key == '0' or Key == ')')
            {
                if (LA_auto == true)
                {
                    PC.printf(") recieved\n");
                    if (la_step == 0.5)
                    {
                        la_step = 1.0;
                    }
                    else if (la_step == 1.0)
                    {
                        la_step = 5.0;
                    }
                    else if (la_step == 5.0)
                    {
                        la_step = 10.0;
                    }
                    else if (la_step == 10.0)
                    {
                        la_step = 15.0;
                    }
                    else if (la_step == 15.0)
                    {
                        la_step = 0.5;
                    }
                    PC.printf("LA Step Size Now\nLA_ST_SZ: %f\n", la_step);
                }
                else
                {                    
                    PC.printf("ERROR: LA in manual mode!\n");
                }
            }
           
            else if (Key == '-' or Key == '_')
            {
                if (LA_auto == true)
                {
                    la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0;
                    PC.printf("- recieved\n");
                    PC.printf("LA auto step size: %f\n", la_step);
                    PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp);
                }
                else
                {
                    PC.printf("ERROR: LA in manual mode!\n"); 
                }
            }
            else if (Key == '=' or Key == '+')
            { 
                if (LA_auto == true)
                {
                    la_setPoint_temp += la_step; //IMU_pitch_angle += 1.0; 
                    PC.printf("+ recieved\n");
                    PC.printf("LA auto step size: %f\n", la_step);
                    PC.printf("LA angle changed to\nLA_ANG: %f\n", la_setPoint_temp);
                }
                else
                {
                    PC.printf("ERROR: LA in manual mode!\n"); 
                }
            }
            else if (Key == 'A' or Key == 'a')
            {
                if (LA_auto == true)
                {
                    PC.printf("A recieved\n");
                    la_setPoint=la_setPoint_temp;
                    PC.printf("LA angle now set to\nLA_A_SND: %f\n", la_setPoint);
                }
                else
                {
                    PC.printf("ERROR: LA in manual mode!\n"); 
                }
            }

            else if (Key == '[' or Key == '{')
            {
                la_P_gain -= 0.1; 
                PC.printf("[ key pressed\n");
                PC.printf("P gain is now %f\n", la_P_gain);
                
            }
            else if (Key == ']' or Key == '}')
            {
                la_P_gain += 0.1;
                PC.printf("] key pressed\n");
                PC.printf("P gain is now %f\n", la_P_gain);
                
            }
            else if (Key == ';')
            {
                la_I_gain -= 0.1;
                PC.printf("; key pressed\n");
                PC.printf("I gain is now %f\n", la_I_gain);
                
            
            }
            else if (Key == '\'')
            {
                la_I_gain += 0.1;
                PC.printf("\\ key pressed\n");
                PC.printf("I gain is now %f\n", la_I_gain);
            }
            else if (Key == '.')
            {
                la_D_gain -= 0.1;
                PC.printf(". key pressed\n");
                PC.printf("D gain is now %f\n", la_D_gain);
            }            
            else if (Key == '/')
            {
                la_D_gain += 0.1; 
                PC.printf("/ key pressed\n");
                PC.printf("D gain is now %f\n", la_D_gain);
            }                

            else if(Key == 'n' or Key == 'N')
            {
                if (LA_auto == false)
                {
                    PC.printf("N key pressed. \n");
                    PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY());
                }
                
                else
                {
                    PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
                }
            }
            else if(Key == 'm' or Key == 'M')
            {
                if (LA_auto == false)
                {
                    PC.printf("M key pressed. \n");
                    PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY());
                }
                
                else
                {
                    PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
                }  
            }
            else if(Key == 'j' or Key == 'J')
            {
                if (LA_auto == false)
                {
                    PC.printf("J key pressed. \n");
                    PC.printf("%s\n", BLA_object.Keyboard_A());
                }
                
                else
                {
                    PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
                }
            }
            else if(Key == 'k' or Key == 'K')
            {
                if (LA_auto == false)
                {
                    PC.printf("K key pressed. \n");
                    PC.printf("%s\n", BLA_object.Keyboard_D());
                }
                
                else
                {
                    PC.printf("ERROR: In LA in Auto Mode, this is a manual command\n");
                }
            }
            
            else if (Key == 't')
            {
                PC.printf("VELOCITY?\n%s\n",BLA_object.get_vel());
            }


            else 
            {
                PC.printf("\n%c received!\n", Key);
                PC.printf("\nDoing nothing.\n");
            }
            
            wait_us(100); //for PC readable
            //PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str());  //get output string
            //BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
        }
    
        //MC readable   
        string MC_readable_string = "";
        MC_readable_string = BLA_object.MC_readable_redux();
        //PC.printf("CHECK_MC_readable:\n%s\n", MC_readable_string);
        
        if (LA_auto == false)
        {
            if (!MC_readable_string.empty())    //if this string is empty
            {
                PC.printf("%s\n", MC_readable_string);     //get responses from the linear actuator motor controller
            }
            else
            {
                ;
                //PC.printf("NOTHING?\n");
            }
        }
        
        //change between automatic and manual mode of linear actuator?             Troy: 7/11/2017
        
        if (la_cases==0)
        {
            if (debug_mode) //debug mode true   
                PC.printf("%s\n", BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str());  //get output string
            else            //debug mode false
                BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
        }
        else if (la_cases==1)
        {
            
            while (count_while==0)
            {
                PC.printf("%s\n", BLA_object.Keyboard_U().c_str());     //velocity = 0, motor disabled
                PC.printf("%s\n", BLA_object.Keyboard_Q().c_str());     //turn off motor
                wait(1);
                PC.printf("%s\n", BLA_object.Keyboard_E().c_str());     //turn on motor
                wait(1);
                PC.printf("\n```````````Linear Actuator in Manual controlled mode````````````\n\n");
                count_while++;
            }
        }
                   
        if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0))
        {
            count ++;
            //pc().printf("We have a small issue\n");
            //PC.printf("posCon().getOutput() %f\n", posCon().getOutput());   //what is this again?
            //always 1?
            if(count==10)
            {
                //PC.printf("> > > Bad pot issue?\n");
                //hBridge().stop();
                count = 0; //reset counter
            }
            
        }
        else if ((5.0*ain.read())<1.0)
        {
            PC.printf("Hit the limit switch??\n");
            hBridge().stop();
        }
        
        
    
        /* buoyancy engine potentiometer string snaps */
        else if (pvf().getVelocity() > 100)
        {
            PC.printf("DEBUG: String position? %f velocity? %f\n", pvf().getPosition(), pvf().getVelocity()); //DEBUG TROY
            //hBridge().stop();
            //PC.printf("PosVelFilter B.E. Velocity: %f\n", pvf().getVelocity());
            PC.printf("********** String broke? *********\n");   
        }

        if (debug_mode)
            PC.printf("+");
            //PC.printf("DEBUG: End of while loop?\n");
    } //end of while loop
}