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