Opgeschoonde code voor verslag
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of TotalCodegr13V2 by
Diff: main.cpp
- Revision:
- 16:bf76ed65da88
- Parent:
- 15:ece330b45d93
- Child:
- 17:416876824d8c
diff -r ece330b45d93 -r bf76ed65da88 main.cpp --- 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;