Diff: main.cpp

Revision:

6:e0857842e8cd

Parent:

5:c510ab61af28

Child:

7:435f984781ab

--- a/main.cpp	Fri Oct 14 08:55:09 2016 +0000
+++ b/main.cpp	Mon Oct 17 06:58:57 2016 +0000
@@ -56,8 +56,6 @@
 
 const double meter_per_count = 1;           // Het aantal meter dat het karretje aflegt per puls, DIT IS NOG ONBEKEND!!!
 
-double error_prev = 0.00000;            // Initiele error derivative waardes
-
 double error1_int = 0.00000;                 // Initiele error integral waardes
 double error2_int = 0.00000;
 
@@ -65,15 +63,18 @@
 
 volatile bool flag1 = false, flag2 = false;   // De flags voor de functies aangeroepen door de tickers
 
-const double Kp = 1.0000000;                    // De PID variablen
-const double Kd = 1.0000000;
-const double Ki = 1.0000000;
+const double Kp_1 = 1.0000000;                    // De PID variablen voor motor 1
+const double Kd_1 = 1.0000000;
+const double Ki_1 = 1.0000000;
 
 const double vrijheid_m1_rad = 0.5*pi;      // Dit is de uiterste postitie vd arm in radialen
 
 double reference_position_motor1 = 0.5*pi;     // Dit is de eerste positie waar de motor heen wil, dit moet dezelfde zijn als de startpositie! 
 
+double PID_output = 0;                      // De eerste PID_output, deze is nul anders gaan de motoren draaien
 
+double error_prev = 0;                      // De initiele previous error
+double error_int = 0;                       // De initiele intergral error
 
 
 
@@ -124,13 +125,13 @@
 
 
 // De PID-controller voor de motors -------------------------------------------------
-double PID_controller(double ref_pos, double mot_pos, double &error_prev, double &error_int, const double Kp, const double Kd, const double Ki){
+double PID_controller(double ref_pos, double mot_pos, double &e_prev, double &e_int, const double Kp, const double Kd, const double Ki){
     double error = ref_pos - mot_pos;       // error uitrekenen
-    double error_dif = (error-error_prev)/T_getpos;      // De error differentieren
+    double error_dif = (error-e_prev)/T_getpos;      // De error differentieren
     //error_dif = .....       // Filter biquad poep
-    error_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
-    error_int = error_int + T_getpos*error;             // De error integreren
-    return Kp*error + Ki*error_int + Kd*error_dif;       // De uiteindelijke PID output
+    e_prev = error;                                 // Hier wordt de error opgeslagen voor de volgende keer
+    e_int = e_int + T_getpos*error;             // De error integreren
+    return Kp*error + Ki*e_int + Kd*error_dif;       // De uiteindelijke PID output
 }
        
 
@@ -185,12 +186,12 @@
     while(safe){            // Draait loop zolang alles veilig is.
         if (flag1){
             flag1 = false;
-            reference_position_motor1 = get_reference_position_m1(vrijheid_m1_rad);
+            PID_output = PID_controller(get_reference_position_m1(vrijheid_m1_rad), get_position_m1(rad_per_count), error_prev, error_int, Kp_1, Kd_1, Ki_1);
             //pc.printf("%f\r\n",get_reference_position_m1);
         }
         if (flag2){
             flag2 = false;
-            set_scope(reference_position_motor1);
+            set_scope(PID_output);
         }
     }