Diff: main.cpp

Revision:

10:09ba965045a7

Parent:

9:697134d3564e

Child:

11:ecd83b303252

--- a/main.cpp	Wed Oct 07 12:40:08 2015 +0000
+++ b/main.cpp	Wed Oct 07 12:53:13 2015 +0000
@@ -50,6 +50,10 @@
     double reference_turn;                  // Set constant to store reference value of the Turn motor
     double position_turn;                   // Set constant to store current position of the Turn motor
     double error;
+    double pwm_to_motor_turn;
+    double pwm_motor_turn_P;
+    double pwm_motor_turn_I;
+    double pwm_motor_turn_D;
     
     //START OF CODE 
     pc.printf("Start of code \n\r");
@@ -98,16 +102,16 @@
         // P-CONTROLLER
         // Calculate error then multiply it with the gain, and store in pwm_to_motor
         
-        error=(reference_turn - position_turn);                             // Current error (input controller)
+        error=(reference_turn - position_turn);                               // Current error (input controller)
         
-   //     integrate_error_turn=integrate_error_turn + sample_time*error;      // integral error output
+        integrate_error_turn=integrate_error_turn + sample_time*error;        // integral error output
 //                                                                                        // overwrite previous integrate error by adding the current error multiplied by the sample time.
 //        
         //double error_derivative_turn=(error - previous_error_turn)/sample_time;    // derivative error output
     
         // FILTER error_derivative_turn (lowpassfilter)
             // biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
-                // const double mT_f_a1=-1.965293372622690e+00;
+                //const double mT_f_a1=-1.965293372622690e+00;
                 //const double mT_f_a2=9.658854605688177e-01;
                 //const double mT_f_b0=1.480219865318266e-04;
                 //const double mT_f_b1=2.960439730636533e-04;
@@ -117,24 +121,27 @@
         //previous_error_turn=error;                                // current error is saved to memory constant to be used in 
                                                                   // the next loop for calculating the derivative error 
                                                                   
-        // double Gain_I_turn=1;
+        double Gain_I_turn=0.01;
         // double Gain_D_turn=1;
 
-        double pwm_motor_turn = error*Gain_P_turn;                     // output P controller to pwm   
+        pwm_to_motor_turn = error*Gain_P_turn;                     // output P controller to pwm   
 
-   //     double pwm_motor_turn_P = error*Gain_P_turn;                     // output P controller to pwm        
-//        double pwm_motor_turn_I = integrate_error_turn*Gain_I_turn;      // output I controller to pwm
+        pwm_motor_turn_P = error*Gain_P_turn;                     // output P controller to pwm        
+        pwm_motor_turn_I = integrate_error_turn*Gain_I_turn;      // output I controller to pwm
 //        double pwm_motor_turn_D = error_derivative_turn*Gain_D_turn;     // output D controller to pwm
+
+        pwm_to_motor_turn = pwm_motor_turn_P + integrate_error_turn*Gain_I_turn;
+
 //        
-//        double pwm_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D; // Total output PID controller to pwm
+//        double pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D; // Total output PID controller to pwm
 //        
         
         // Keep Pwm between -1 and 1
-        keep_in_range(&pwm_motor_turn, -1,1);                     // Pass to motor controller but keep it in range!
-        pc.printf("pwm %f \n\r", pwm_motor_turn);
+        keep_in_range(&pwm_to_motor_turn, -1,1);                     // Pass to motor controller but keep it in range!
+        pc.printf("pwm %f \n\r", pwm_to_motor_turn);
 
         // Check error and decide direction to turn
-        if(pwm_motor_turn > 0)
+        if(pwm_to_motor_turn > 0)
             {
             motordirection_turn=ccw;
             pc.printf("if loop pwm_to_motor > 0 \n\r");
@@ -146,7 +153,7 @@
             }
         
         // Put pwm_motor to the motor
-        pwm_motor_turn=(abs(pwm_motor_turn));
+        pwm_motor_turn=(abs(pwm_to_motor_turn));
     }
 }
 }