Diff: speedlimiter.cpp

Revision:

6:1a4ed88c9a4b

Parent:

5:aef1fc6c0df1

Child:

7:d636d073b942

--- a/speedlimiter.cpp	Thu Oct 25 17:43:21 2018 +0000
+++ b/speedlimiter.cpp	Thu Oct 25 22:30:50 2018 +0000
@@ -93,32 +93,68 @@
     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
-    const double b0 = 3312.0;
-    const double b1 = -3061.0;
-    const double a1 = -1.0;
+    double var = readAdcTest();
+    double Kp =  18000 ; //3.1848e+03
+    double Ki = 4300.0; // 2.5181e+03
 
     // valeurs anterieures
-    static double lastE = 0.0;
-    static double lastOut = 0.0;
+    static double ie = 0.0;
 
     // calculs
     double Vm = getMeasuredSpeed();
-    double Vd = getReferenceSpeed();
+    double Vd = 40 * var;
     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;
+        double e = Vd - Vm;
+        const double dt = TRANSFER_FUNCTION_PERIOD;
+        ie = ie + e*dt;
+        
+        out = Kp*e + Ki*ie;
 
         // out est maintenant en 'force'
         out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
@@ -127,63 +163,207 @@
             pc->printf("Acquisition start:\n\r");
             first_acquisition = false;
         }
-//        pc->printf("%f;", Vm);
-//        wait(5);
-
     } else {
-        lastE = 0.00000000001;
-        lastOut = 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);
+    pc->printf("Vm: %.2f\t Vd: %.2f\t Eh: %.2f\t Kp: %.2f\tVar: %.2f\n\r", Vm, Vd, out, Ki, var);
 
     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()
 //{
-//    // valeurs anterieures
-//    static double x1 = 0.0;
-//    static double y1 = 0.0;
+//    static bool first_acquisition = true;
 //
-//    // constantes calcules avec matlab
-//    const double b0 = 10793.0;
-//    const double b1 =  -8513.5;
+//    // constantes calcules dans MATLAB
+//    const double b0 = 3312.0;
+//    const double b1 = -3061.0;
 //    const double a1 = -1.0;
 //
-//    const double TRANS_k_gear = 20.5; //Gearbox ratio
-//    const double TRANS_eff = 0.91;  // Efficiency of the transmission
-//    const double TRANS_R_wheel = 0.3175; // Radius of the wheel [m]
-//    const double TRANS_k = TRANS_k_gear/TRANS_R_wheel; // Global ratio of the transmission [m]
+//    // 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;
 //
-//    const double G_forceToTorque = (1/TRANS_k) * TRANS_eff;
+//        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
-//    const float Vm = getMeasuredSpeed();
-//    const float Vd = getReferenceSpeed();
-//    double y0 = 0.0;
+//    double Vm = getMeasuredSpeed();
+//    double Vd = getReferenceSpeed();
+//    double out = 0.0;
 //    if (readAdcPedalHi() > 1) {
-//        const double x0 = Vd - Vm;
-//        y0 = ( (-a1*y1) + (b0*x0) + (b1*x1) ) * G_forceToTorque;
+//        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;
 //
-//        const double deltaT = 20.0;
-//        const double deltaE = PEDAL_HI_MAX_VALUE - PEDAL_HI_MIN_VALUE;
-//        const double ratioTtoE = deltaE / deltaT / 159.63 * deltaE;
-//        y0 = y0 * ratioTtoE + PEDAL_HI_MIN_VALUE;
+//        // out est maintenant en 'force'
+//        out = (out / 133240.0 * (PEDAL_HI_MAX_VALUE-PEDAL_HI_MIN_VALUE)) + PEDAL_HI_MIN_VALUE;
 //
-//        // update des valeurs anterieurs
-//        x1 = x0;
-//        y1 = y0;
+//        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: %f\t Vd: %f\t Eh: %f\t El\n\r", Vm, Vd, y0, y0/2);
+//    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;
+//    }
 //
-//    return (float)y0;
+//    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)
 {