Diff: main.cpp

Revision:

3:1257a7d2eb3a

Parent:

2:c3cb3ea3c9fa

Child:

4:3c22d85a94a8

--- a/main.cpp	Tue Jun 13 17:23:06 2017 +0000
+++ b/main.cpp	Fri Jun 30 18:39:25 2017 +0000
@@ -20,17 +20,12 @@
 
 string IMU_STRING = "";
 
-DigitalOut pin_ENA(p23);    //ENA       //DigitalOut PinName (pin) //Troy: IF WE NEED IT
-DigitalOut pin_IN1(p22);    //IN1      (changed from pin11 and 12)   
-DigitalOut pin_IN2(p21);    //IN2       //should I use wait or interrupt to control this?
 
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
-DigitalOut enab(p9);        // a/d converter enable
-
 AnalogIn   pressure_analog_in(A5); //Initialize pin20   (read is float value)
 AnalogIn   ain(p18);
 
@@ -40,27 +35,36 @@
 Ticker PRESSURE_ticker;
 
 Ticker BCE_ticker;  //new 6/5/17
-/*float motor_cmd = 0.0;
-float positionCmd = 250.0;
-float P = 0.10;
-float I = 0.00;
-float D = 0.00;
-*/
+Ticker PID_ticker;  //new 6/14/17
+Ticker LA_ticker;   //new 6/22/17
+
 float positionCmd = 250.0;
 /* ************************************************************************* */ 
 
+/* 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;
+
 void IMU_ticking()
 {
     led1 = !led1;   //flash the IMU LED
-    
-    if (!IMU_STRING.empty())
-        PC.printf("%s\n", IMU_STRING);  //if there's something there, print it
+
+    PC.printf("%s\n", IMU_STRING.c_str());  //if there's something there, print it
 }
 
 void PRESSURE_ticking()
 {
-    PC.printf("Pressure sensor (PSI)\nPRESS_PSI: %3.3f\n", 3.3*1.503*10 * pressure_analog_in.read());    //read the analog pin
-    //this voltage has been checked and scaled properly (5/19/2017)
+    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)
 }
 
 void BCE_ticking()  //new 6/5/17
@@ -70,13 +74,25 @@
 
 int main()
 {  
-    PC.baud(115200);  //mbed to PC serial connection speed
+    PC.baud(9600);  //mbed to PC serial connection speed
     //PC.baud(230400);
     //got screwy when i changed it
     hBridge().stop();
-    //start up the system timer so the position velocity estimator has a time signal
-    systemTime().start();
-    //enab=1;
+    
+    PC.printf("Linear Actuator PID Program Started 6/22/17\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();
@@ -94,6 +110,9 @@
     float count = 0.0;
     //char userInput;   //from Trent's code?
 
+    int la_step = 1;
+    int la_setPoint_temp = 0;
+
     hBridge().run(motor_cmd);
     
     //set the intial gains for the position controller
@@ -113,27 +132,29 @@
     //PC.printf("Hit shift + \"H\" to home the battery Linear Actuator\n");
     
     /* *************************** Potentiometer *************************** */
-    PRESSURE_ticker.attach(&PRESSURE_ticking, 2.0);
+    //PRESSURE_ticker.attach(&PRESSURE_ticking, 1.0);
     /* *************************** Potentiometer *************************** */
     
     /* *************************** MOTOR CONTROLLER *************************** */
-    Battery_Linear_Actuator BLA_object;               //create the IMU object from the imported class
+    //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, 2.0);
+    //IMU_ticker.attach(&IMU_ticking, 5.0);
     /* *************************** IMU *************************** */
     
     /* *************************** BCE *************************** */
-    float previous_positionCmd = -1;
-    BCE_ticker.attach(&BCE_ticking, 2.0);
+    //float previous_positionCmd = -1;
+    //BCE_ticker.attach(&BCE_ticking, 5.0);
     /* *************************** BCE *************************** */
     
     while(1) 
     {
         /* *************************** 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 *************************** */
         
         /*          Buoyancy Engine         */
@@ -152,19 +173,11 @@
         //PC.printf("WHILE LOOP\n");  //DEBUG
         if (PC.readable())
         {
-            //PC.printf("PC IS READABLE\n");  //DEBUG
-            Key=PC.getc(); 
-            if(Key=='a' or Key =='A')
-            {
-                PC.printf("%s\n", BLA_object.Keyboard_A());
-            }
+            //led4 != led4;
+            //PC.printf("DEBUG: PC IS READABLE\n");  //DEBUG
             
-            else if(Key=='d' or Key =='D')
-            {
-                PC.printf("%s\n", BLA_object.Keyboard_D());
-            }
-
-            else if(Key=='w' or Key =='W')
+            Key=PC.getc(); 
+            if(Key=='w' or Key =='W')
             {
                 PC.printf("\nW received!\n");
                 
@@ -179,37 +192,96 @@
                 count = 0;
 
             }
+            
+            /*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')  //motor disabled
+            /*else if(Key =='Q' or Key == 'q')  //motor disabled
             {
                 PC.printf("%s\n", BLA_object.Keyboard_Q());
             } 
             
-            else if(Key =='E')  //motor enabled
+            else if(Key =='E' or Key == 'e')  //motor enabled
             {
                 PC.printf("%s\n", BLA_object.Keyboard_E());
-            }
+            }*/
             
-            else if(Key=='-' or Key =='_')
+            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      
+                // "-999999" means it is not working 
+            }
+            else if (Key == 'A' or Key == 'a')
             {
-                PC.printf("%s\n", BLA_object.Keyboard_DASH_KEY());         //decrease step size
-            } 
-            
-            else if(Key=='+' or Key=='=')
+                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 == '_')
             {
-                PC.printf("%s\n", BLA_object.Keyboard_EQUAL_KEY());        //increase step size
+                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); 
+            }
+            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); 
             }
-            
-            else if(Key==']')  //100 to 30000, INCREASE (100 increments)
+            else if (Key == '0' or Key == ')')
             {
-                PC.printf("%s\n", BLA_object.Keyboard_RIGHT_BRACKET());    //motor speed
+                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; 
+                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("P gain is now %f\n", la_P_gain);
+                
             
-            else if(Key=='[')  //30000 to 100, DECREASE (100 increments)
-            {
-                PC.printf("%s\n", BLA_object.Keyboard_LEFT_BRACKET());     //motor speed
             }
-            
+            else if (Key=='\'')
+                la_I_gain += 0.1;
+            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");
@@ -222,7 +294,12 @@
             
             else if(Key =='H')  //homing sequence
             {
-                PC.printf("%s\n", BLA_object.Keyboard_H());     //home linear actuator manually
+                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       
             }
                 //check command against desired control buttons
             else if (Key == 'z' or Key =='Z') {
@@ -243,11 +320,15 @@
             }
             
             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();
+        }            
         
+        //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(BLA_object.MC_readable())   //if you can read the motor controller do this...
+        /*if(BLA_object.MC_readable())   //if you can read the motor controller do this...
         {
             //PC.printf("BATTERY LINEAR ACTUATOR");
             
@@ -281,7 +362,7 @@
         {
             //hBridge().stop();
             pc().printf("********** String broke? *********\n");   
-        }
+        }*/
                 
     }
 }
\ No newline at end of file