JEncoderPWMAbsolute.h

Go to the documentation of this file.

#ifndef J_ENCODER_ATTACH_INTERRUPT_PWM_ABSOLUTE_H
#define J_ENCODER_ATTACH_INTERRUPT_PWM_ABSOLUTE_H
#include "JEncoder.h"
#include <Arduino.h>
 
#define JEncoderPWMAbsolute_AS5048settings \
    {                                      \
        4095, 4119, 16, HIGH, 800, 1100    \
    }
 
class JEncoderPWMAbsolute : public JEncoder {
public:
    struct pwmSettings {
        uint16_t RESOLUTION;
        uint16_t PWM_STEPS;
        uint16_t MIN_STEPS;
        bool dataState;
        uint16_t MIN_CYCLE;
        uint16_t MAX_CYCLE;
    };
 
protected:
    byte encoderPin;
 
private:
    long turns;
    uint16_t angle;
    uint16_t lastAngle;
    uint16_t zeroPos;
    bool resetAngleOnStart;
    bool firstAngle;
    int8_t reverse;
    float distPerCountFactor;
    bool newSpeed;
    uint16_t lastVelAngle;
    long lastVelTurns;
    unsigned long lastVelTimeMicros;
    unsigned long velEnoughTime;
    unsigned long velEnoughTicks;
    float velocity;
    bool justStartedVel;
    volatile bool newAngle;
    volatile unsigned long dataStartMicros;
    volatile unsigned long dataEndMicros;
    volatile unsigned long earlyDataStartMicros;
    volatile unsigned long oldDataEndMicros;
    bool setPosZero;
 
    pwmSettings ps;
 
public:
    JEncoderPWMAbsolute(byte _encoderPin, pwmSettings _ps, bool _reverse = false, float _distPerCountFactor = 1.0, unsigned long _velEnoughTime = 0, unsigned long _velEnoughTicks = 0, bool _resetAngleOnStart = false, uint16_t _zeroPos = 0)
        : ps(_ps)
    {
        firstAngle = true;
        resetAngleOnStart = _resetAngleOnStart;
        encoderPin = _encoderPin;
        if (_reverse) {
            reverse = -1;
        } else {
            reverse = 1;
        }
        setDistPerCountFactor(_distPerCountFactor);
 
        velEnoughTime = _velEnoughTime;
        velEnoughTicks = _velEnoughTicks;
        zeroPos = _zeroPos % ps.RESOLUTION;
 
        turns = 0;
        angle = 0;
        lastAngle = 0;
        newSpeed = false;
        lastVelAngle = 0;
        lastVelTurns = 0;
        lastVelTimeMicros = 0;
        velocity = 0.0;
        newAngle = false;
        dataStartMicros = 0;
        dataEndMicros = 0;
        earlyDataStartMicros = 0;
        oldDataEndMicros = 0;
        justStartedVel = true;
        setPosZero = false;
    }
 
    virtual void setUpInterrupts(void (*_isrPointer)(void));
    virtual void turnOffInterrupts();
 
    int rawReading()
    {
        return angle * reverse;
    }
 
    long intTurns()
    {
        return turns * reverse;
    }
 
    long zeroCounter()
    {
        return zeroCounter(true);
    }
 
    long zeroCounter(bool resetAngle)
    {
        long tTurns = turns;
        turns = 0;
        if (resetAngle) {
            setPosZero = true;
        }
        justStartedVel = true;
        return (tTurns * ps.RESOLUTION) * reverse;
    }
 
    float getVel()
    {
        return velocity;
    }
 
    long getCounter()
    {
        return (turns * ps.RESOLUTION + angle) * reverse;
    }
 
    float getPos()
    {
        return (turns * ps.RESOLUTION + angle) * reverse * distPerCountFactor;
    }
 
    float getDistPerCountFactor()
    {
        return distPerCountFactor;
    }
 
    void setDistPerCountFactor(float _factor)
    {
        distPerCountFactor = _factor / ps.RESOLUTION;
    }
 
    bool hasDirection()
    {
        return true;
    }
 
    bool isVelNew()
    {
        return newSpeed;
    }
 
    bool setZeroPos(uint16_t zeroAngle)
    {
        if (zeroAngle % ps.RESOLUTION != zeroPos) {
            zeroPos = zeroAngle % ps.RESOLUTION;
            return true;
        }
        return false;
    }
 
    void run()
    {
        if (newAngle) {
            noInterrupts();
            long interval = dataEndMicros - dataStartMicros;
            long cycle = dataEndMicros - oldDataEndMicros;
            newAngle = false;
            interrupts();
            if (interval > 0 && cycle > ps.MIN_CYCLE && cycle < ps.MAX_CYCLE) { //check that interrupts didn't change one number but not the other, and that frequency is within range
                if ((resetAngleOnStart && firstAngle) || setPosZero) {
                    zeroPos = 0;
                }
                angle = (constrain(interval * ps.PWM_STEPS / cycle - ps.MIN_STEPS, 0, ps.RESOLUTION - 1) + ps.RESOLUTION - zeroPos) % ps.RESOLUTION;
                if ((resetAngleOnStart && firstAngle) || setPosZero) {
                    zeroPos = (angle) % ps.RESOLUTION;
                    firstAngle = false;
                    lastAngle = 0;
                    setPosZero = false;
                    angle = 0;
                }
                if (abs((int16_t)angle - (int16_t)lastAngle) >= ps.RESOLUTION / 2) { // angle jump over half of circle is assummed to be the shorter crossing of 0
                    if (angle > lastAngle) {
                        turns--;
                    } else {
                        turns++;
                    }
                }
                lastAngle = angle;
            }
        }
        int64_t velDist = ((int16_t)angle - (int16_t)lastVelAngle) + (turns - lastVelTurns) * ps.RESOLUTION;
        if ((micros() - lastVelTimeMicros > velEnoughTime || abs(velDist) > velEnoughTicks)) {
            if (justStartedVel) {
                justStartedVel = false;
                lastVelAngle = angle;
                velDist = 0;
            }
            velocity = (double)1000000.0 * velDist / (micros() - lastVelTimeMicros) * distPerCountFactor * reverse;
            newSpeed = true;
            lastVelTimeMicros = micros();
            lastVelAngle = angle;
            lastVelTurns = turns;
        } else {
            newSpeed = false;
        }
    }
    void encoderISR()
    {
        if (digitalRead(encoderPin) == ps.dataState) {
            earlyDataStartMicros = micros();
        } else {
            oldDataEndMicros = dataEndMicros;
            dataEndMicros = micros();
            dataStartMicros = earlyDataStartMicros;
            newAngle = true;
        }
    }
};
#endif