Diff: main.cpp

Revision:

4:3c22d85a94a8

Parent:

3:1257a7d2eb3a

Child:

5:7421776f6b08

--- a/main.cpp	Fri Jun 30 18:39:25 2017 +0000
+++ b/main.cpp	Fri Jul 07 21:23:19 2017 +0000
@@ -39,8 +39,11 @@
 Ticker LA_ticker;   //new 6/22/17
 
 float positionCmd = 250.0;
+
 /* ************************************************************************* */ 
 
+float pi = 3.14159265359;
+
 /* PID LOOP STUFF */
 float la_setPoint = 0.00;       //the IMU pitch angle we want (setpoint)
 
@@ -58,12 +61,13 @@
 {
     led1 = !led1;   //flash the IMU LED
 
-    PC.printf("%s\n", IMU_STRING.c_str());  //if there's something there, print it
+    //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);
 }
 
 void PRESSURE_ticking()
 {
-    PC.printf("pressure: %f mm \r", (0.00122*(adc().ch1_filt)*14.931)-0.0845);    //read the analog pin
+    //PC.printf("pressure: %f mm \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)
 }
 
@@ -110,8 +114,20 @@
     float count = 0.0;
     //char userInput;   //from Trent's code?
 
-    int la_step = 1;
-    int la_setPoint_temp = 0;
+    float la_step = 1.0;
+    float la_setPoint_temp = 0.0;
+    
+    bool BCE_auto = true;
+    bool LA_auto = true;
+
+    float bce_auto_step_raw = 1.0;
+    float bce_auto_step_l;
+    float convert = 10000;
+    float bce_auto_step_ml = bce_auto_step_raw * convert;
+    int bce_man_step = 1;
+    float volume_bce = 90.0*convert;
+    float positionCmd_temp;
+    float ml_to_l= 0.000000001;
 
     hBridge().run(motor_cmd);
     
@@ -129,10 +145,14 @@
     /* *************************** LED *************************** */
     
     PC.printf("Program Started 6/5/17\n");
+    int 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_ticker.attach(&PRESSURE_ticking, 1.0);
+    PRESSURE_ticker.attach(&PRESSURE_ticking, 3.0);
     /* *************************** Potentiometer *************************** */
     
     /* *************************** MOTOR CONTROLLER *************************** */
@@ -141,12 +161,12 @@
     
     /* *************************** IMU *************************** */
     IMU_code IMU_object;               //create the IMU object from the imported class
-    //IMU_ticker.attach(&IMU_ticking, 5.0);
+    IMU_ticker.attach(&IMU_ticking, 3.0);
     /* *************************** IMU *************************** */
     
     /* *************************** BCE *************************** */
     //float previous_positionCmd = -1;
-    //BCE_ticker.attach(&BCE_ticking, 5.0);
+    BCE_ticker.attach(&BCE_ticking, 3.0);
     /* *************************** BCE *************************** */
     
     while(1) 
@@ -176,44 +196,269 @@
             //led4 != led4;
             //PC.printf("DEBUG: PC IS READABLE\n");  //DEBUG
             
-            Key=PC.getc(); 
-            if(Key=='w' or Key =='W')
+            Key=PC.getc();
+        //Universal MBED Controls 
+            if(Key=='!')  //RESET THE MBED
             {
-                PC.printf("\nW received!\n");
-                
-                posCon().writeSetPoint(positionCmd);
-                //posCon().setPgain(P);
-                //posCon().setIgain(I);
-                //posCon().setDgain(D);
-                hBridge().run(posCon().getOutput());
-                
-                hBridge().reset();
-                
-                count = 0;
-
+                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 == 'k' or Key=='K')
-                PC.printf("%s\n", BLA_object.Keyboard_K().c_str());
-            else if (Key == 'l' or Key == 'L')
-                PC.printf("%s\n", BLA_object.Keyboard_L().c_str());*/
-              
-            /*else if(Key =='Q' or Key == 'q')  //motor disabled
+            else if(Key =='H')  //homing sequence
             {
-                PC.printf("%s\n", BLA_object.Keyboard_Q());
-            } 
-            
-            else if(Key =='E' or Key == 'e')  //motor enabled
-            {
-                PC.printf("%s\n", BLA_object.Keyboard_E());
-            }*/
-            
+                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
+                wait(1); //for debugging       
+            }
             else if(Key=='p' or Key == 'P')
             {
                 PC.printf("### position is %s ###\n", BLA_object.get_pos().c_str());     //flip this back and forth
-                wait(10); //for debugging      
+                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 Manual 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 Automatic 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 = 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); 
+                }
+                else
+                {
+                    PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
+                
+                }
+            }
+            else if(Key =='d' or Key == 'D')
+            {
+                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     
+                }
+                else
+                {
+                    PC.printf("ERROR: In BCE Manual Mode, this is a auto command\n");
+                }
+            }
+            else if(Key == 'f' or Key == 'F')
+            {
+                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 
+                }
+                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");
+                    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());
+                
+                    hBridge().reset();
+                
+                    count = 0;
+                }
+                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_man_step == 1)
+                    {
+                        bce_man_step = 10;
+                    }
+                    else if (bce_man_step == 10)
+                    {
+                        bce_man_step = 25;
+                    }
+                    else if (bce_man_step == 25)
+                    {
+                        bce_man_step = 50;
+                    }
+                    else if (bce_man_step == 50)
+                    {
+                        bce_man_step = 1;
+                    }
+
+                    PC.printf("BCE Manual Step Size Now %d\n", bce_man_step);
+                }
+                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')
+            {
+                if (BCE_auto == false)
+                {
+                    PC.printf("\nW received!\n");
+                    PC.printf("BASETP: %3.0f\n", positionCmd);
+                    posCon().writeSetPoint(positionCmd);
+                    //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("```````````Now in IMU Controlled Mode```````````````\n");
+                    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("```````````Now in Manual Mode````````````````````\n");
+                    cases = 1;
+                    count_while = 0;
+                }
+                else 
+                {
+                    PC.printf("ERROR: LA already in manual mode\n");
+                }
+            }
+            else if (Key == '0' or Key == ')')
+            {
+                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 %f\n", la_step);
+            }
+            
+
             else if (Key == 'A' or Key == 'a')
             {
                 PC.printf("A recieved\n");
@@ -224,35 +469,15 @@
             {
                 la_setPoint_temp -= la_step; //IMU_pitch_angle -= 1.0;
                 PC.printf("- recieved\n");
-                PC.printf("LA angle changed to: %d  la_step: %d\n", la_setPoint_temp, la_step); 
+                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: %d\n", la_setPoint_temp); 
+                PC.printf("LA angle changed to: %f\n", la_setPoint_temp); 
             }
-            else if (Key == '0' or Key == ')')
-            {
-                PC.printf(") recieved\n");
-                if (la_step == 1)
-                {
-                    la_step = 5;
-                }
-                else if (la_step == 5)
-                {
-                    la_step = 10;
-                }
-                else if (la_step == 10)
-                {
-                    la_step = 15;
-                }
-                else if (la_step == 15)
-                {
-                    la_step = 1;
-                }
-                PC.printf("LA Step Size Now %d\n", la_step);
-            }
+
             else if (Key=='[' or Key == '{')
             {
                 la_P_gain -= 0.1; 
@@ -270,48 +495,51 @@
             else if (Key==';')
             {
                 la_I_gain -= 0.1;
-                PC.printf("[ key pressed\n");
-                PC.printf("P gain is now %f\n", la_P_gain);
+                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;
-            else if (Key=='/')
-                la_D_gain += 0.1; 
-                
-            else if(Key=='!')  //RESET THE MBED
             {
-                PC.printf("MBED RESET KEY (!) PRESSED\n");
-                PC.printf("Linear Actuator Motor disabled! Please re-enable.\n");
-                //disable the motor
-                BLA_object.Keyboard_Q();        //DISABLE THE MOTOR
-                wait(0.5);                      //500 milliseconds
-                mbed_reset(); //reset the mbed!
-            }
-            
-            else if(Key =='H')  //homing sequence
+                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')
             {
-                PC.printf("### homing the device ###");
-                BLA_object.Keyboard_H();
-                wait(10); //for debugging
-                
-                PC.printf("### position is %d ###\n", BLA_object.get_pos().c_str());     //flip this back and forth
-                wait(10); //for debugging       
+                PC.printf("N key pressed. \n");
+                PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY());
+            }
+            else if(Key == 'm' or Key == 'M')
+            {
+                PC.printf("M key pressed. \n");
+                PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY());  
             }
-                //check command against desired control buttons
-            else if (Key == 'z' or Key =='Z') {
-                //increment the duty cycle
-                positionCmd += 10.0 ;
-                PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
-            } 
-            else if (Key == 'x' or Key == 'X') {
-                //decrement the duty cycle
-                positionCmd -= 10.0 ;
-                PC.printf("The position for the buoyancy motor is\nBEP: %3.0f\n", positionCmd); //to read in MATLAB
+            else if(Key == 'j' or Key == 'J')
+            {
+                PC.printf("J key pressed. \n");
+                PC.printf("%s\n", BLA_object.Keyboard_A());
             }
+            else if(Key == 'k' or Key == 'K')
+            {
+                PC.printf("K key pressed. \n");
+                PC.printf("%s\n", BLA_object.Keyboard_D());
+            }
+
 
             else 
             {
@@ -324,10 +552,28 @@
             //BLA_object.PID_velocity_control(la_setPoint, IMU_pitch_angle, la_P_gain, la_I_gain, la_D_gain).c_str();
         }            
         
-        //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 (cases==0)
+        {
+            //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)
+        }
+        else if (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(BLA_object.MC_readable())   //if you can read the motor controller do this...
         {
             //PC.printf("BATTERY LINEAR ACTUATOR");
@@ -339,7 +585,7 @@
 //                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 ++;
@@ -347,7 +593,7 @@
                 if(count==10)
                 {
                     pc().printf("Bad pot issue\n");
-                    hBridge().stop();
+                    //hBridge().stop();
                 }
             
         }
@@ -362,7 +608,7 @@
         {
             //hBridge().stop();
             pc().printf("********** String broke? *********\n");   
-        }*/
+        }
                 
     }
 }
\ No newline at end of file