Line data    Source code

       1              : #pragma once
       2              : 
       3              : #include <AH/STL/type_traits>
       4              : #include <Settings/NamespaceSettings.hpp>
       5              : #include <stdint.h>
       6              : 
       7              : BEGIN_CS_NAMESPACE
       8              : 
       9              : /// Class to keep track of relative position changes of rotary encoders.
      10              : template <class Enc_t>
      11              : class EncoderState {
      12              :   private:
      13              :     using SignedEnc_t = typename std::make_signed<Enc_t>::type;
      14              :     int16_t speedMultiply;
      15              :     uint8_t pulsesPerStep;
      16              :     int16_t remainder = 0;
      17              :     Enc_t deltaOffset = 0;
      18              : 
      19              :   public:
      20           21 :     EncoderState(int16_t speedMultiply, uint8_t pulsesPerStep)
      21           21 :         : speedMultiply(speedMultiply), pulsesPerStep(pulsesPerStep) {}
      22              : 
      23           35 :     int16_t update(Enc_t encval) {
      24              :         // If Enc_t is an unsigned type, integer overflow is well-defined, which
      25              :         // is what we want when Enc_t is small and expected to overflow.
      26              :         // However, we need it to be signed because we're interested in the
      27              :         // delta.
      28           35 :         Enc_t uDelta = encval - deltaOffset;
      29           35 :         if (uDelta == 0)
      30            2 :             return 0;
      31           33 :         int16_t delta = static_cast<SignedEnc_t>(uDelta);
      32              :         // Assumption: delta and speedMultiply are relatively small, so
      33              :         // multiplication probably won't overflow.
      34           33 :         int16_t multipliedDelta = delta * speedMultiply + remainder;
      35           33 :         int16_t scaledDelta = multipliedDelta / pulsesPerStep;
      36           33 :         remainder = multipliedDelta % pulsesPerStep;
      37           33 :         deltaOffset += uDelta;
      38           33 :         return scaledDelta;
      39              :     }
      40              :     void setSpeedMultiply(int16_t speedMultiply) {
      41              :         // TODO: Is this correct? Is it necessary? What with negative speedMult?
      42              :         remainder = remainder * speedMultiply / this->speedMultiply;
      43              :         this->speedMultiply = speedMultiply;
      44              :     }
      45              :     int16_t getSpeedMultiply() const { return this->speedMultiply; }
      46              : };
      47              : 
      48              : END_CS_NAMESPACE