Diff: speedlimiter.cpp

Revision:

4:a8c9f6a13633

Parent:

3:4da392d2bae8

Child:

5:aef1fc6c0df1

--- a/speedlimiter.cpp	Thu Oct 18 17:31:03 2018 +0000
+++ b/speedlimiter.cpp	Tue Oct 23 14:19:54 2018 +0000
@@ -5,6 +5,7 @@
 #include "speedlimiter.hpp"
 
 #define debug SpeedLimiter::pc->printf
+
 Serial* SpeedLimiter::pc = new Serial(USBTX, USBRX);
 
 SpeedLimiter::SpeedLimiter(const PinName& pedalInHi, const PinName& pedalInLo, const PinName& pedalOutHi, const PinName& pedalOutLo)
@@ -14,36 +15,26 @@
     , _pedalOutLo(pedalOutLo)
     , _referenceSpeed(DISABLE_ECO_ALGO_TRIGGER)
     , _measuredSpeed(0.0)
-    , _ticker(new Ticker)
-    , _mutex(new Mutex)
 {
 }
 
 SpeedLimiter::~SpeedLimiter()
 {
-    delete _ticker;
-    delete _mutex;
-}
-
-void SpeedLimiter::start()
-{
-    _ticker->attach(callback(this, &SpeedLimiter::tickerCallback), TRANSFER_FUNCTION_PERIOD);
-}
-
-void SpeedLimiter::tickerCallback()
-{
-    ipControllerTransferFunction();
 }
 
 void SpeedLimiter::ipControllerTransferFunction()
 {
     // write voltages at beginning of function to prevent jitter
     // voltage will be delayed by 1 call which is okay.
-    writeAdcPedalHi(getOutputPedalVoltageHi());
-    writeAdcPedalLo(getOutputPedalVoltageLo());
-    
-    debug("hello world\n\r");
-    
+//    pc->printf("H\n\r");
+    const float voutHi = getOutputPedalVoltageHi();
+    const float voutLo = getOutputPedalVoltageLo();
+
+//    pc->printf("Hi: %f\t Lo: %f\n\r", voutHi, voutLo);
+
+    writeAdcPedalHi(voutHi);
+    writeAdcPedalLo(voutLo);
+
     // calculate voltage for next call
     const float referenceSpeed = getReferenceSpeed();
     float outputAdcVoltageHi = 0;
@@ -55,8 +46,11 @@
         outputAdcVoltageHi = ecoEnabledAlgorithm();
     }
 
+//    outputAdcVoltageHi = ADC_OUTPUT_MAX_VALUE / 2;
     outputAdcVoltageLo = outputAdcVoltageHi / 2;
 
+//    pc->printf("tmpHi: %f\t tmpLo: %f\n\r", outputAdcVoltageHi, outputAdcVoltageLo);
+
     setOutputPedalVoltageHi(outputAdcVoltageHi);
     setOutputPedalVoltageLo(outputAdcVoltageLo);
 }
@@ -102,30 +96,38 @@
 float SpeedLimiter::ecoEnabledAlgorithm()
 {
     // valeurs anterieures
-    static float x1 = 0.0;
-    static float y1 = 0.0;
+    static double x1 = 0.0;
+    static double y1 = 0.0;
 
     // constantes calcules avec matlab
-    const float b0 = 10793.0;
-    const float b1 =  -8513.5;
-    const float a1 = -1.0;
+    const double b0 = 10793.0;
+    const double b1 =  -8513.5;
+    const double a1 = -1.0;
 
-    const float TRANS_k_gear = 20.5; //Gearbox ratio
-    const float TRANS_eff = 0.91;  // Efficiency of the transmission
-    const float TRANS_R_wheel = 0.3175; // Radius of the wheel [m]
-    const float TRANS_k = TRANS_k_gear/TRANS_R_wheel; // Global ratio of the transmission [m]
+    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 float G_forceToTorque = (1/TRANS_k) * TRANS_eff;
+    const double G_forceToTorque = (1/TRANS_k) * TRANS_eff;
 
     // calculs
-    const float x0 = getMeasuredSpeed();
-    const float y0 = ( (-a1*y1) + (b0*x0) + (b1*x1) ) * G_forceToTorque;
+    double y0 = 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;
 
-    // update des valeurs anterieurs
-    x1 = x0;
-    y1 = y0;
+        // update des valeurs anterieurs
+        x1 = x0;
+        y1 = y0;
+    }
 
-    return 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'