Revision 5:7421776f6b08, committed 2017-07-17

Comitter:

tnhnrl

Date:

Mon Jul 17 14:28:53 2017 +0000

Parent:

4:3c22d85a94a8

Child:

6:ce2cf7f4d7d5

Commit message:

7 14 version
;

Changed in this revision

--- a/Battery_Linear_Actuator.lib	Fri Jul 07 21:23:19 2017 +0000
+++ b/Battery_Linear_Actuator.lib	Mon Jul 17 14:28:53 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/mdavis30/code/Battery_Linear_Actuator/#ca2454ef80d9
+https://developer.mbed.org/users/tnhnrl/code/Battery_Linear_Actuator_7_14/#627fd46c2b99

--- a/IMU_code.lib	Fri Jul 07 21:23:19 2017 +0000
+++ b/IMU_code.lib	Mon Jul 17 14:28:53 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/mdavis30/code/IMU_code/#0672f84101e4
+https://developer.mbed.org/users/tnhnrl/code/IMU_code_7_14/#b00adea2187b

--- a/LTC1298.lib	Fri Jul 07 21:23:19 2017 +0000
+++ b/LTC1298.lib	Mon Jul 17 14:28:53 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/mdavis30/code/LTC1298/#45e4bd71be62
+https://developer.mbed.org/users/tnhnrl/code/LTC1298_7_14/#5ae1f8b2d173

--- a/PosVelFilter.lib	Fri Jul 07 21:23:19 2017 +0000
+++ b/PosVelFilter.lib	Mon Jul 17 14:28:53 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/mdavis30/code/PosVelFilter/#ec4117689673
+https://developer.mbed.org/users/tnhnrl/code/PosVelFilter_7_14/#992e774dc62a

--- a/System.lib	Fri Jul 07 21:23:19 2017 +0000
+++ b/System.lib	Mon Jul 17 14:28:53 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/mdavis30/code/System/#9764b0bd8cb9
+https://developer.mbed.org/users/tnhnrl/code/System_7_14/#66f2b18a16a4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Troy_Notes.txt	Mon Jul 17 14:28:53 2017 +0000
@@ -0,0 +1,33 @@
+Troy: On July 13th I recalibrated the a/d converter and it should be pretty close.
+
+Troy: 14th, seems like you still have to check the position of the thing when you send a command, it doesn't always fully command the piston
+
+Troy: 14th, There is a weird bug where the motor likes to move when it's sitting stationary a few mm back and forth.
+
+Troy: 14th, What was the pressure sensor calibrated off of?
+
+Troy: pos controller active generates crazy non-zero numbers but that's useful: e.g. 1610612736, -536870912
+
+MISC NOTES:
+
+//HIT SEND TWICE!  Why does it work this way?
+
+//BCE has about 16.125 inches long = 409.575 mm length of travel to switch
+
+//commanded position of -50 ==> 385 mm withdrawn
+//commanded position of -25 ==> 384 mm, what?
+//commanded position of 50  ==> 345 mm
+//commanded position of -25 ==> 385 mm, what? max position?
+//commanded position of 50  ==> 340 mm
+//commanded position of 0   ==> 385 mm, what...?
+//commanded position of 50  ==> 340 mm
+//commanded position of 100 ==> 296 mm
+//commanded position of 350 ==> 60 mm
+//commanded position of 300 ==> 80 mm
+//commanded position of 400 ==> -2.5 mm (piston sticks out)
+//commanded position of 50 ==>  315 mm (piston sticks out)
+//commanded position of 25 ==>  361 mm (piston sticks out) //not accurate right now
+
+//i'm stopping it at 370 mm
+//had some bs where i hit the limit switch before i retracted fully when i did position 0
+//don't go back that far
\ No newline at end of file

--- a/main.cpp	Fri Jul 07 21:23:19 2017 +0000
+++ b/main.cpp	Mon Jul 17 14:28:53 2017 +0000
@@ -8,6 +8,8 @@
 #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)
@@ -38,7 +40,7 @@
 Ticker PID_ticker;  //new 6/14/17
 Ticker LA_ticker;   //new 6/22/17
 
-float positionCmd = 250.0;
+float positionCmd = 200.0;  //250
 
 /* ************************************************************************* */ 
 
@@ -57,23 +59,31 @@
 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
-
-    //PC.printf("%s\n", IMU_STRING.c_str());  //if there's something there, print it
-   // PC.printf("\t pitch angle... %f    set pitch angle: %f\n", IMU_pitch_angle, la_setPoint);
+    
+    if (debug_mode)
+        PC.printf("%s\n", IMU_STRING.c_str());  //if there's something there, print it
 }
 
 void PRESSURE_ticking()
 {
-    //PC.printf("pressure: %f mm \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845);    //read the analog pin
+    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
 {
-    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());
+    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()
@@ -83,7 +93,9 @@
     //got screwy when i changed it
     hBridge().stop();
     
-    PC.printf("Linear Actuator PID Program Started 6/22/17\n");
+    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
         
@@ -117,17 +129,21 @@
     float la_step = 1.0;
     float la_setPoint_temp = 0.0;
     
-    bool BCE_auto = true;
-    bool LA_auto = true;
+    //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 = 10000;
+    float convert = 1;
     float bce_auto_step_ml = bce_auto_step_raw * convert;
-    int bce_man_step = 1;
-    float volume_bce = 90.0*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;
+    //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);
     
@@ -135,25 +151,27 @@
     posCon().setPgain(P);
     posCon().setIgain(I);
     posCon().setDgain(D);
-    //posCon().writeSetPoint(positionCmd);
+    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 = 1;
+    led3 = 0;
     led4 = 1;    
     /* *************************** LED *************************** */
-    
-    PC.printf("Program Started 6/5/17\n");
-    int cases = 0;
+
+    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");
     
-    /* *************************** Potentiometer *************************** */
+    /* ************************** Pressure Sensor ************************** */
     PRESSURE_ticker.attach(&PRESSURE_ticking, 3.0);
-    /* *************************** Potentiometer *************************** */
+    /* ************************** Pressure Sensor ************************** */
     
     /* *************************** MOTOR CONTROLLER *************************** */
     //Battery_Linear_Actuator BLA_object;               //create the IMU object from the imported class
@@ -166,15 +184,25 @@
     
     /* *************************** BCE *************************** */
     //float previous_positionCmd = -1;
-    BCE_ticker.attach(&BCE_ticking, 3.0);
+    //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?
-        //PC.printf("pitch angle... %f    set pitch angle: %f\n", IMU_pitch_angle, la_setPoint); 
+        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         */
@@ -190,15 +218,13 @@
         //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);
     
-        //PC.printf("WHILE LOOP\n");  //DEBUG
         if (PC.readable())
         {
-            //led4 != led4;
-            //PC.printf("DEBUG: PC IS READABLE\n");  //DEBUG
+            led4 = !led4;           //read indicator changes
+            Key=PC.getc();
             
-            Key=PC.getc();
         //Universal MBED Controls 
-            if(Key=='!')  //RESET THE MBED
+            if(Key == '!')  //RESET THE MBED
             {
                 PC.printf("MBED RESET KEY (!) PRESSED\n");
                 PC.printf("Linear Actuator Motor disabled!\n");
@@ -207,18 +233,47 @@
                 wait(0.5);                      //500 milliseconds
                 mbed_reset(); //reset the mbed!
             }
-            else if(Key =='H')  //homing sequence
+            
+            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
                 
-                PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str());     //flip this back and forth
+                ////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')
+            else if(Key == 'p' or Key == 'P')
             {
-                PC.printf("### position is %s ###\n", BLA_object.get_pos().c_str());     //flip this back and forth
+                // 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 
             }
@@ -233,7 +288,7 @@
                 }
                 else
                 {
-                    PC.printf("BCE: Still in Manual Mode\n");
+                    PC.printf("BCE: Still in Automatic Mode\n");
                 }
             }
             else if (Key == '.' or Key == '>')
@@ -245,7 +300,7 @@
                 }
                 else
                 {
-                    PC.printf("BCE: Still in Automatic Mode\n");
+                    PC.printf("BCE: Still in Manual Mode\n");
                 }
             }
         //BCE Automatic Controls
@@ -264,10 +319,18 @@
                     }
                     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 %7.0f milliliters\n", bce_auto_step_ml); 
+                    PC.printf("BCE Auto Step Size Now\n BE_ST_ML: %7.0f milliliters\n", bce_auto_step_ml); 
                 }
                 else
                 {
@@ -275,12 +338,23 @@
                 
                 }
             }
-            else if(Key =='d' or Key == 'D')
+            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)
                 {
-                    volume_bce -= bce_auto_step_ml;
-                    PC.printf("The volume for the buoyancy motor is\nVBE: %1.5f liters\n", volume_bce*ml_to_l); //to read in MATLAB     
+                    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
                 {
@@ -289,10 +363,21 @@
             }
             else if(Key == 'f' or Key == 'F')
             {
+                PC.printf("(f) volume_bce: %f\n", volume_bce);
                 if (BCE_auto == true)
                 {
-                    volume_bce += bce_auto_step_ml;
-                    PC.printf("The volume for the buoyancy motor is\nVBE: %1.5f liters\n", volume_bce*ml_to_l); //to read in MATLAB 
+                    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
                 {
@@ -304,19 +389,19 @@
                 if (BCE_auto == true)
                 {
                     PC.printf("\nR received!\n");
-                    positionCmd=(volume_bce-(pi*40.64*40.64*377))/(-1*pi*40.64*40.64);
-                    //positionCmd= positionCmd_temp*0.000000001;
-                    //PC.printf("BCE engine going to position: %3.2f\n", positionCmd);
-                    PC.printf("\nBASETP: %3.0f\n", positionCmd);
-                    posCon().writeSetPoint(positionCmd);
-                    //posCon().setPgain(P);
-                    //posCon().setIgain(I);
-                    //posCon().setDgain(D);
-                    hBridge().run(posCon().getOutput());
+                    
+                    //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();
+                    hBridge().reset();                      //reset to start this process of moving the h-bridge
                 
-                    count = 0;
+                    count = 0;                              //not sure...
                 }
                 else
                 {
@@ -329,63 +414,88 @@
                 if (BCE_auto == false)
                 {
                     PC.printf("BCE Manual Step Size Change\n");
-                    if (bce_man_step == 1)
+                    if (bce_manual_step == 1)
                     {
-                        bce_man_step = 10;
+                        bce_manual_step = 10;
                     }
-                    else if (bce_man_step == 10)
+                    else if (bce_manual_step == 10)
                     {
-                        bce_man_step = 25;
+                        bce_manual_step = 25;
                     }
-                    else if (bce_man_step == 25)
+                    else if (bce_manual_step == 25)
                     {
-                        bce_man_step = 50;
+                        bce_manual_step = 50;
                     }
-                    else if (bce_man_step == 50)
+                    else if (bce_manual_step == 50)
                     {
-                        bce_man_step = 1;
+                        bce_manual_step = 1;
                     }
 
-                    PC.printf("BCE Manual Step Size Now %d\n", bce_man_step);
+                    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 == 'z' or Key =='Z') 
-            {
-                if (BCE_auto == false)
-                {
-                    //increment the duty cycle
-                    positionCmd -= bce_man_step;
-                    PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
-                }
-                else
-                {
-                    PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
-                }
-            }
-            else if (Key == 'x' or Key == 'X') 
-            {
-                if (BCE_auto == false)
-                {
-                    //decrement the duty cycle
-                    positionCmd += bce_man_step;
-                    PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
-                }
-                else
-                {
-                    PC.printf("ERROR: In BCE Auto Mode, this is a manual command\n");
-                }
-            }
-            else if(Key=='w' or Key =='W')
+            else if(Key == 'w' or Key == 'W')
             {
                 if (BCE_auto == false)
                 {
                     PC.printf("\nW received!\n");
-                    PC.printf("BASETP: %3.0f\n", positionCmd);
-                    posCon().writeSetPoint(positionCmd);
+                    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);
@@ -412,8 +522,8 @@
                 else
                 {
                     LA_auto = true;
-                    PC.printf("```````````Now in IMU Controlled Mode```````````````\n");
-                    cases = 0;
+                    PC.printf("```````````LA now in IMU (Auto) Controlled Mode```````````````\n");
+                    la_cases = 0;
                     count_while = 0;
                 }
             }
@@ -423,76 +533,106 @@
                 {
                     LA_auto = false;
                     //Change cases: go from imu controlled to manual
-                    PC.printf("```````````Now in Manual Mode````````````````````\n");
-                    cases = 1;
+                    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 == ')')
             {
-                PC.printf(") recieved\n");
-                if (la_step == 0.5)
+                if (LA_auto == true)
                 {
-                    la_step = 1.0;
+                    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 if (la_step == 1.0)
-                {
-                    la_step = 5.0;
-                }
-                else if (la_step == 5.0)
-                {
-                    la_step = 10.0;
+                else
+                {                    
+                    PC.printf("ERROR: LA in manual mode!\n");
                 }
-                else if (la_step == 10.0)
+            }
+           
+            else if (Key == '-' or Key == '_')
+            {
+                if (LA_auto == true)
                 {
-                    la_step = 15.0;
+                    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 if (la_step == 15.0)
+                else
                 {
-                    la_step = 0.5;
+                    PC.printf("ERROR: LA in manual mode!\n"); 
                 }
-                PC.printf("LA Step Size Now %f\n", la_step);
             }
-            
-
+            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')
             {
-                PC.printf("A recieved\n");
-                la_setPoint=la_setPoint_temp;
-                PC.printf("LA angle now set to %f\n", la_setPoint);
-             }   
-            else if (Key=='-' or Key == '_')
-            {
-                la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0;
-                PC.printf("- recieved\n");
-                PC.printf("LA angle changed to: %d  la_step: %f\n", la_setPoint_temp, la_step); 
-            }
-            else if (Key =='=' or Key == '+')
-            { 
-                la_setPoint_temp += la_step; //IMU_pitch_angle += 1.0; 
-                PC.printf("+ recieved\n");
-                PC.printf("LA angle changed to: %f\n", la_setPoint_temp); 
+                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 == '{')
+            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 == '}')
+            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==';')
+            else if (Key == ';')
             {
                 la_I_gain -= 0.1;
                 PC.printf("; key pressed\n");
@@ -500,19 +640,19 @@
                 
             
             }
-            else if (Key=='\'')
+            else if (Key == '\'')
             {
                 la_I_gain += 0.1;
-                PC.printf("\ key pressed\n");
+                PC.printf("\\ key pressed\n");
                 PC.printf("I gain is now %f\n", la_I_gain);
             }
-            else if (Key=='.')
+            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=='/')
+            else if (Key == '/')
             {
                 la_D_gain += 0.1; 
                 PC.printf("/ key pressed\n");
@@ -521,23 +661,60 @@
 
             else if(Key == 'n' or Key == 'N')
             {
-                PC.printf("N key pressed. \n");
-                PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY());
+                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')
             {
-                PC.printf("M key pressed. \n");
-                PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY());  
+                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')
             {
-                PC.printf("J key pressed. \n");
-                PC.printf("%s\n", BLA_object.Keyboard_A());
+                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')
             {
-                PC.printf("K key pressed. \n");
-                PC.printf("%s\n", BLA_object.Keyboard_D());
+                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());
             }
 
 
@@ -550,16 +727,36 @@
             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 (cases==0)
+        if (LA_auto == false)
         {
-            //PC.printf("Case is 0; IMU control\n");
-            //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();
-            //wait_us(100); //for PC readable (0.1 ms)
+            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");
+            }
         }
-        else if (cases==1)
+        
+        //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)
@@ -574,41 +771,39 @@
             }
         }
                    
-        /*if(BLA_object.MC_readable())   //if you can read the motor controller do this...
-        {
-            //PC.printf("BATTERY LINEAR ACTUATOR");
-            
-            
-            //PC.printf("Motor Controller response:\n");
-//            while(MC.readable())
-//            {
-//                PC.putc(MC.getc()); //this is a pass-through of the MC (getc) to the PC (putc)
-//                wait_ms(1); //1000, 10, 20, 100 (needed at least 1 ms, verified through testing)
-//            }
-        }*/
         if ((abs(pvf().getVelocity())<0.1) && (posCon().getOutput()>0.0))
         {
             count ++;
             //pc().printf("We have a small issue\n");
-                if(count==10)
-                {
-                    pc().printf("Bad pot issue\n");
-                    //hBridge().stop();
-                }
+            //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");
+            PC.printf("Hit the limit switch??\n");
             hBridge().stop();
         }
+        
+        
     
-        //string snaps
+        /* 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("********** String broke? *********\n");   
+            //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
 }
\ No newline at end of file