Diff: speedlimiter.cpp

Revision:

7:d636d073b942

Parent:

6:1a4ed88c9a4b

Child:

8:51549d7108dd

--- a/speedlimiter.cpp	Thu Oct 25 22:30:50 2018 +0000
+++ b/speedlimiter.cpp	Fri Oct 26 17:50:47 2018 +0000
@@ -93,64 +93,26 @@
     return value;
 }
 
-//// Proportionnel 3s
-//float SpeedLimiter::ecoEnabledAlgorithm()
-//{
-//    static bool first_acquisition = true;
-//
-//    // constantes calcules dans MATLAB
-//    double Kp =  1.6783e+03; //3.1848e+03
-//
-//    // valeurs anterieures
-//    static double ie = 0.0;
-//
-//    // calculs
-//    double Vm = getMeasuredSpeed();
-//    double Vd = getReferenceSpeed();
-//    double out = 0.0;
-//    if (readAdcPedalHi() > 1) {
-//        double e = Vd - Vm;
-//        const double dt = TRANSFER_FUNCTION_PERIOD;
-//        ie = ie + e*dt;
-//        
-//        out = Kp*e;
-//
-//        // out est maintenant en 'force'
-//        out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
-//
-//        if(first_acquisition) {
-//            pc->printf("Acquisition start:\n\r");
-//            first_acquisition = false;
-//        }
-//    } else {
-//        ie = 0.00000000001;
-//        first_acquisition = true;
-//    }
-//
-//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
-//
-//    return (float)out;
-//}
-
-// PI 3s (meilleur a date)
 float SpeedLimiter::ecoEnabledAlgorithm()
 {
     static bool first_acquisition = true;
     
-    // constantes calcules dans MATLAB
+    // constants calibrated in vehicul
     double var = readAdcTest();
-    double Kp =  18000 ; //3.1848e+03
-    double Ki = 4300.0; // 2.5181e+03
+    double Kp =  18000 ;
+    double Ki = 4300.0;
 
     // valeurs anterieures
     static double ie = 0.0;
 
     // calculs
     double Vm = getMeasuredSpeed();
-    double Vd = 40 * var;
+    double Vd = 20;
     double out = 0.0;
     if (readAdcPedalHi() > 1) {
         double e = Vd - Vm;
+        double eMax = 4;
+        if(e > eMax) e = eMax;
         const double dt = TRANSFER_FUNCTION_PERIOD;
         ie = ie + e*dt;
         
@@ -173,197 +135,6 @@
     return (float)out;
 }
 
-//// essai routier: lim = 10 kmh, overshoot de 50%, un seul ronflement
-//// max 15 kmh, min 7 kmh.
-//// temps de stabilisation 3s
-//float SpeedLimiter::ecoEnabledAlgorithm()
-//{
-//    static bool first_acquisition = true;
-//
-//    // constantes calcules dans MATLAB
-//    const double b0 = 3312.0;
-//    const double b1 = -3061.0;
-//    const double a1 = -1.0;
-//
-//    // valeurs anterieures
-//    static double lastE = 0.0;
-//    static double lastOut = 0.0;
-//
-//    // calculs
-//    double Vm = getMeasuredSpeed();
-//    double Vd = getReferenceSpeed();
-//    double out = 0.0;
-//    if (readAdcPedalHi() > 1) {
-//        const double e = Vd - Vm;
-//
-//        // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
-//        out = (b0 * e) + (b1 * lastE) - (a1 * lastOut);
-//
-//        lastE = e;
-//        lastOut = out;
-//
-//        // out est maintenant en 'force'
-//        out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
-//
-//        if(first_acquisition) {
-//            pc->printf("Acquisition start:\n\r");
-//            first_acquisition = false;
-//        }
-//    } else {
-//        lastE = 0.00000000001;
-//        lastOut = 0.00000000001;
-//        first_acquisition = true;
-//    }
-//
-//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
-//
-//    return (float)out;
-//}
-
-//// temps de stabilisation 1s
-//// oscillation, grand overshoot (20 kmh), consigne 10 kmh
-//float SpeedLimiter::ecoEnabledAlgorithm()
-//{
-//    static bool first_acquisition = true;
-//
-//    // constantes calcules dans MATLAB
-//    const double b0 = 10790.0;
-//    const double b1 = -8514.0;
-//    const double a1 = -1.0;
-//
-//    // valeurs anterieures
-//    static double lastE = 0.0;
-//    static double lastOut = 0.0;
-//
-//    // calculs
-//    double Vm = getMeasuredSpeed();
-//    double Vd = getReferenceSpeed();
-//    double out = 0.0;
-//    if (readAdcPedalHi() > 1) {
-//        const double e = Vd - Vm;
-//
-//        // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
-//        out = (b0 * e) + (b1 * lastE) - (a1 * lastOut);
-//
-//        lastE = e;
-//        lastOut = out;
-//
-//        // out est maintenant en 'force'
-//        out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
-//
-//        if(first_acquisition) {
-//            pc->printf("Acquisition start:\n\r");
-//            first_acquisition = false;
-//        }
-//    } else {
-//        lastE = 0.00000000001;
-//        lastOut = 0.00000000001;
-//        first_acquisition = true;
-//    }
-//
-//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
-//
-//    return (float)out;
-//}
-
-//// implementation du PID 2s de stab
-//// overshoot significatif, ronflement constant
-//float SpeedLimiter::ecoEnabledAlgorithm()
-//{
-//    static bool first_acquisition = true;
-//
-//    // constantes calcules dans MATLAB
-//    const double b0 = 4.246e04;
-//    const double b1 = -7.958e04;
-//    const double b2 =  3.768e04;
-//    const double a1 = -1.0;
-//
-//    // valeurs anterieures
-//    static double lastE = 0.0;
-//    static double lastlastE = 0.0;
-//    static double lastOut = 0.0;
-//    static double lastlastOut = 0.0;
-//
-//    // calculs
-//    double Vm = getMeasuredSpeed();
-//    double Vd = getReferenceSpeed();
-//    double out = 0.0;
-//    if (readAdcPedalHi() > 1) {
-//        const double e = Vd - Vm;
-//
-//        // y(n) = b0 * x(n) + b1 * x(n-1) - a1 * y(n-1)
-//        out = (b0 * e) + (b1 * lastE) + (b2 * lastlastE) - (a1 * lastOut);
-//
-//        lastlastE = lastE;
-//        lastE = e;
-//        lastlastOut = lastOut;
-//        lastOut = out;
-//
-//        // out est maintenant en 'force'
-//        out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
-//
-//        if(first_acquisition) {
-//            pc->printf("Acquisition start:\n\r");
-//            first_acquisition = false;
-//        }
-//    } else {
-//        lastE = 0.00000000001;
-//        lastOut = 0.00000000001;
-//        first_acquisition = true;
-//    }
-//
-//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
-//
-//    return (float)out;
-//}
-
-//// overshoot ~17, oscillation / ronflement
-//float SpeedLimiter::ecoEnabledAlgorithm()
-//{
-//    static bool first_acquisition = true;
-//
-//    // constantes calcules dans MATLAB
-//    double Kp =  4.7772e+03;
-//    double Ki = 5.6658e+03;
-//    double Kd = 4000.0;
-//
-//    // valeurs anterieures
-//    static double ie = 0.0;
-//    static double lastE = 0.0;
-//
-//    // calculs
-//    double Vm = getMeasuredSpeed();
-//    double Vd = getReferenceSpeed();
-//    double out = 0.0;
-//    if (readAdcPedalHi() > 1) {
-//        double e = Vd - Vm;
-//        const double dt = TRANSFER_FUNCTION_PERIOD;
-//        ie = ie + e*dt;
-//        double de = (e - lastE)/dt;
-//        
-//        out = Kp*e + Ki*ie + Kd*de;
-//        
-//        lastE = e;
-//
-//        // out est maintenant en 'force'
-//        out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
-//
-//        if(first_acquisition) {
-//            pc->printf("Acquisition start:\n\r");
-//            first_acquisition = false;
-//        }
-//    } else {
-//        lastE = 0.00000000001;
-//        ie = 0.00000000001;
-//        first_acquisition = true;
-//    }
-//
-//    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t El: %.2f\n\r", Vm, Vd, out, out/2);
-//
-//    return (float)out;
-//}
-
-
 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)
 {