Diff: main.cpp

Revision:

16:bf76ed65da88

Parent:

15:ece330b45d93

Child:

17:416876824d8c

--- a/main.cpp	Mon Nov 02 14:03:45 2015 +0000
+++ b/main.cpp	Mon Nov 02 14:06:12 2015 +0000
@@ -45,67 +45,7 @@
 const double c = 4200; // Aantal counts 1 rotatie
 const double q = g/c;
 
-//PI-controller constante
-const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1
-const double motor1_Ki = 0.002; // Integrating gain m1.
-const double motor1_Ts = 0.01; // Time step m1
-const double motor2_Kp = 2.0; // Dit is de proportionele gain motor 1
-const double motor2_Ki = 0.002; // Integrating gain m1.
-const double motor2_Ts = 0.01; // Time step m1
-double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1
-double err_int_m2 = 0 ; // De integrating error op het beginstijdstip m1
 
-// Reusable P controller
-double Pc1 (double error1, const double motor1_Kp)
-{
-    return motor1_Kp * err_int_m1;
-}
-double Pc2 (double error2, const double motor2_Kp)
-{
-    return motor2_Kp * err_int_m2;
-}
-
-// Measure the error and apply output to the plant
-void motor1_controlP()
-{
-    double referenceP1 = PotMeter1.read();
-    double positionP1 = q*motor1.getPosition();
-    double motorP1 = Pc1(referenceP1 - positionP1, motor1_Kp);
-}
-
-void motor2_controlP()
-{
-    double referenceP2 = PotMeter2.read();
-    double positionP2 = q*motor2.getPosition();
-    double motorP2 = Pc2(referenceP2 - positionP2, motor2_Kp);
-}
-
-// Reusable PI controller
-double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int_m1)
-{
-    err_int_m1 = err_int_m1 * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken
-    return motor1_Kp*error + motor1_Ki*err_int_m1;
-} // De totale fout die wordt hersteld met behulp van PI control.
-
-double PI2 (double error2, const double motor2_Kp, const double motor2_Ki, const double motor2_Ts, double &err_int_m2)
-{
-    err_int_m2 = err_int_m2 * motor2_Ts*error2;
-    return motor2_Kp*error2 + motor2_Ki*err_int_m2;
-}
-
-void motor1_controlPI()
-{
-    double referencePI1 = PotMeter1.read();
-    double positionPI1 = q*motor1.getPosition();
-    double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1);
-}
-
-void motor2_controlPI()
-{
-    double referencePI2 = PotMeter2.read();
-    double positionPI2 = q*motor2.getPosition();
-    double motorPI2 = PI(referencePI2 - positionPI2, motor2_Kp, motor2_Ki, motor2_Ts, err_int_m2);
-}
 
 // Filter1 = High pass filter tot 20 Hz
 double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;