From 36b86899f9d281d176fd41119da4f0a68af7893a Mon Sep 17 00:00:00 2001 From: mane9 Date: Tue, 2 Apr 2024 18:38:23 +0300 Subject: [PATCH] Improved regentravel calculations. (#42) Use historical potnom values to increase and regenrampstr to decrease regentravel. --- include/throttle.h | 1 + src/stm32_sine.cpp | 1 + src/throttle.cpp | 85 ++++++++++++++++++++++++++++++++++------------ 3 files changed, 65 insertions(+), 22 deletions(-) diff --git a/include/throttle.h b/include/throttle.h index fe60f86..19c8903 100644 --- a/include/throttle.h +++ b/include/throttle.h @@ -55,6 +55,7 @@ class Throttle static float idleThrotLim; static float cruiseThrotLim; static float regenRamp; + static float regenrampstr; static float throttleRamp; static int bmslimhigh; static int bmslimlow; diff --git a/src/stm32_sine.cpp b/src/stm32_sine.cpp index 14db1ed..97a0156 100644 --- a/src/stm32_sine.cpp +++ b/src/stm32_sine.cpp @@ -303,6 +303,7 @@ void Param::Change(Param::PARAM_NUM paramNum) Throttle::brknom = Param::GetFloat(Param::regentravel); Throttle::brknompedal = Param::GetFloat(Param::brakeregen); Throttle::regenRamp = Param::GetFloat(Param::regenramp); + Throttle::regenrampstr = Param::GetFloat(Param::regenrampstr); Throttle::maxregentravelhz = Param::GetFloat(Param::maxregentravelhz); Throttle::brkmax = Param::GetFloat(Param::offthrotregen); Throttle::brkcruise = Param::GetFloat(Param::cruiseregen); diff --git a/src/throttle.cpp b/src/throttle.cpp index 0634fab..23c5fe9 100644 --- a/src/throttle.cpp +++ b/src/throttle.cpp @@ -53,6 +53,7 @@ float Throttle::fmax; int Throttle::accelmax; int Throttle::accelflt; float Throttle::maxregentravelhz; +float Throttle::regenrampstr; bool Throttle::CheckAndLimitRange(int* potval, int potIdx) { @@ -84,48 +85,88 @@ float Throttle::DigitsToPercent(int potval, int potidx) return (100 * (potval - potmin[potidx])) / (potmax[potidx] - potmin[potidx]); } + +/** + * Calculate throttle and regen. + * - Increase regentravel when lifting accelerator using historical pedal values. + * - Decrease regentravel when speed goes below regenrampstr. + * + * Parameters: + * potnom = accelerator pedal pressed percentage + * pot2nom = optional potentiometer for adjusting regen braking + * brkpedal = is brake pedal pressed + * rotorfreq = rotor rotation freqyency in hz + * + * Used variables: + * brkmax = regen percentage to apply, when brake is pressed + * brknompedal = regen percentage to apply, when accelerator is lifted + * brknom = percentage of how much of accelerator is assigned for adjusting regen + * regenrampstr = rotor hz where regen strenght start to decrease linearly +*/ float Throttle::CalcThrottle(float potnom, float pot2nom, bool brkpedal, float rotorfreq) { - static float brknomDynamic = 0.01; - static float pedalPressPercentage = 0.0f; - pedalPressPercentage = potnom; + // variables for rolling average calculation + static const int history = 20; // 20 events, 10ms delta = 200ms of history to calculate + static int currentArrayIndex = 0; // pointer where to pu the next value + static float prevPotnom[history] = {}; // array holding previous potnom value + static float potnomSums = 0; // sum of values in array - float scaledBrkMax = brkpedal ? brknompedal : brkmax; + // current dynamic regentravel + static float currentRegentravel = 0.0; - //Never reach 0, because that can spin up the motor + float scaledBrkMax = brkpedal ? brknompedal : brkmax; + // Never reach 0, because that can spin up the motor scaledBrkMax = -0.1 + (scaledBrkMax * pot2nom) / 100.0f; - - //maxregentravelhz (speed) gives max brknomDynamic (brknom) - float speedPercentage = MIN((rotorfreq / maxregentravelhz), 1.0); - //set max regen by speed - if (brknomDynamic < brknom * speedPercentage) + // increase speedBasedRegenTravel linearly until rotorfreq reaches maxregentravelhz, then stay at max. + // Don't go over max regentravel (brknom). + float speedBasedRegenTravel = MIN(brknom * rotorfreq / maxregentravelhz, brknom); + + // decreasing regentravel when rotor hz is goes below "regen ramp start" + if (rotorfreq < regenrampstr) { - brknomDynamic = speedPercentage * brknom; + float decreasingRegenTravel = brknom * rotorfreq / regenrampstr; + // decrease only if it's been higher before (don't start to increase on low speeds to keep linear curve) + if (currentRegentravel > decreasingRegenTravel) + { + currentRegentravel = decreasingRegenTravel; + } } - //reset to full pedal travel when stopped and pedal 0% on throttle - if (potnom <= 0.1 && rotorfreq < 0.1) + // increase currentRegenTravel when lifting accelerator + if (currentRegentravel < speedBasedRegenTravel) // if currentregentravel is smaller than speedBaseRegenTravel { - brknomDynamic = 0.01; + // current pedal position delta compared against average of last 200ms + float acceleratorDelta = potnom - potnomSums / history; + if (acceleratorDelta < 0) // accelerator is being lifted (acceleratorDelta is negative) + { + currentRegentravel = RAMPUP(currentRegentravel, speedBasedRegenTravel, -acceleratorDelta); + } } - if (brkpedal) + if (brkpedal) // give max brake regen { potnom = scaledBrkMax; } - else if (potnom < brknomDynamic) //calculate regen + else if (potnom < currentRegentravel) // calculate regen { - potnom -= brknomDynamic; - potnom = -(potnom * scaledBrkMax / brknomDynamic); + potnom -= currentRegentravel; + potnom = -(potnom * scaledBrkMax / currentRegentravel); } - else //calculate thorttle + else // calculate thorttle { - potnom -= brknomDynamic; - potnom = 100.0f * potnom / (100.0f - brknomDynamic); + potnom -= currentRegentravel; + potnom = 100.0f * potnom / (100.0f - currentRegentravel); } - return potnom > pedalPressPercentage ? pedalPressPercentage : potnom; + // there next 4 lines are for calculating rolling average for historical pedal (potnom) position + // and saving current potnom + potnomSums -= prevPotnom[currentArrayIndex]; // remove old potnom value from average calculation sums + potnomSums += potnom; // replace it with new potnom value + prevPotnom[currentArrayIndex] = potnom; // put current potnom to array + currentArrayIndex = (currentArrayIndex + 1) % history; // loop array index from 0 - history, start again from 0 + + return potnom; } float Throttle::CalcThrottleBiDir(float potval, bool brkpedal)