Diff: speedlimiter.cpp

Revision:

5:aef1fc6c0df1

Parent:

4:a8c9f6a13633

Child:

6:1a4ed88c9a4b

--- a/speedlimiter.cpp	Tue Oct 23 14:19:54 2018 +0000
+++ b/speedlimiter.cpp	Thu Oct 25 17:43:21 2018 +0000
@@ -95,40 +95,94 @@
 
 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]
-
-    const double G_forceToTorque = (1/TRANS_k) * TRANS_eff;
+    // valeurs anterieures
+    static double lastE = 0.0;
+    static double lastOut = 0.0;
 
     // calculs
-    double y0 = 0.0;
+    double Vm = getMeasuredSpeed();
+    double Vd = getReferenceSpeed();
+    double out = 0.0;
     if (readAdcPedalHi() > 1) {
 
-        const float Vm = getMeasuredSpeed();
-        const float Vd = getReferenceSpeed();
-        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;
+
+        // 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;
+        }
+//        pc->printf("%f;", Vm);
+//        wait(5);
+
+    } 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;
+}
+
+
 
-    return (float)y0;
-}
+//float SpeedLimiter::ecoEnabledAlgorithm()
+//{
+//    // valeurs anterieures
+//    static double x1 = 0.0;
+//    static double y1 = 0.0;
+//
+//    // constantes calcules avec matlab
+//    const double b0 = 10793.0;
+//    const double b1 =  -8513.5;
+//    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]
+//
+//    const double G_forceToTorque = (1/TRANS_k) * TRANS_eff;
+//
+//    // calculs
+//    const float Vm = getMeasuredSpeed();
+//    const float Vd = getReferenceSpeed();
+//    double y0 = 0.0;
+//    if (readAdcPedalHi() > 1) {
+//        const double x0 = Vd - Vm;
+//        y0 = ( (-a1*y1) + (b0*x0) + (b1*x1) ) * G_forceToTorque;
+//
+//        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;
+//
+//        // update des valeurs anterieurs
+//        x1 = x0;
+//        y1 = y0;
+//    }
+//
+//    pc->printf("Vm: %f\t Vd: %f\t Eh: %f\t El\n\r", Vm, Vd, y0, y0/2);
+//
+//    return (float)y0;
+//}
 
 // Returns 'value' bounded between 'lowerBound' and 'upperBound'
 float SpeedLimiter::boundValue(float value, const float lowerBound, const float upperBound)