JEncoderAS5600I2C.h

Go to the documentation of this file.

#ifndef JENCODER_AS5600I2C_H
#define JENCODER_AS5600I2C_H
#include "JEncoder.h"
#include <Arduino.h>
#include <Wire.h>
class JEncoderAS5600I2C : public JEncoder {
private:
    TwoWire* wire;
 
    byte address;
    long turns;
    uint16_t angle;
    int8_t reverse;
    float velocity;
    float distPerCountFactor;
    bool newSpeed;
    uint16_t lastAngle;
    uint16_t lastVelAngle;
    long lastVelTurns;
    unsigned long lastVelTimeMicros;
    unsigned long velEnoughTime;
    unsigned long velEnoughTicks;
    bool recognizeOutOfRange;
    uint16_t zeroAngle;
 
public:
    const byte ZMCO_REG = 0x00;
    const byte ZPOSMSB_REG = 0x01;
    const byte MPOSMSB_REG = 0X03;
    const byte MANG_REG = 0x05;
    const byte CONF_REG = 0x07;
    const byte RAWANGLEMSB_REG = 0x0C;
    const byte ANGLEMSB_REG = 0x0E;
    const byte STATUS_REG = 0x0B;
    const byte AGC_REG = 0x1A;
    const byte MAGNITUDE_REG = 0x1B;
    const byte BURN_REG = 0xFF;
 
    void writeRegister8(uint8_t reg, uint8_t value)
    {
        wire->beginTransmission(address);
        wire->write(reg);
        wire->write(value);
        wire->endTransmission();
    }
 
    void writeRegister12(uint8_t reg, uint16_t value)
    {
        // 12 bit value got from 2 8bits registers (7..0 MSB + 3..0 LSB) => 12 bits value
        wire->beginTransmission(0x36);
        wire->write(reg);
        wire->write((uint8_t)((value >> 8) & 0b1111));
        wire->write((uint8_t)(value & 0b11111111));
        wire->endTransmission();
    }
 
    uint16_t readRegister12(uint8_t reg)
    {
        // 12 bit value got from 2 8bits registers (11..8 MSB + 7..0 LSB) => 12 bits value
        byte readArray[2];
 
        wire->beginTransmission(0x36);
        wire->write(reg);
        wire->endTransmission(false);
 
        wire->requestFrom(0x36, 2);
        readArray[0] = wire->read();
        readArray[1] = wire->read();
 
        uint16_t readValue = ((readArray[0] & 0b1111) << 8) | readArray[1];
        return readValue;
    }
 
    uint8_t readRegister8(uint8_t reg)
    {
        uint8_t readValue;
 
        wire->beginTransmission(address);
        wire->write(reg);
        wire->endTransmission(false);
 
        wire->requestFrom(address, (uint8_t)1);
        readValue = (uint8_t)wire->read();
 
        return readValue;
    }
 
    const unsigned int STEPS_PER_TURN = 4096; // resolution of encoder
    static const byte AS5048B_DEFAULT_ADDRESS = 0x36; // can be accessed as JEncoderAS5048bI2C::AS5048B_DEFAULT_ADDRESS
 
    JEncoderAS5600I2C(bool _reverse = false, float _distPerCountFactor = 1.0, uint16_t _zeroAngle = 0, unsigned long _velEnoughTime = 0, unsigned long _velEnoughTicks = 0, bool _recognizeOutOfRange = true)
    {
        address = AS5048B_DEFAULT_ADDRESS;
        wire = &Wire;
        if (_reverse) {
            reverse = -1;
        } else {
            reverse = 1;
        }
        turns = 0;
        angle = 0;
        velocity = 0;
        setDistPerCountFactor(_distPerCountFactor);
        newSpeed = false;
        lastAngle = 0;
        lastVelAngle = 0;
        lastVelTurns = 0;
        lastVelTimeMicros = 0;
        velEnoughTime = _velEnoughTime;
        velEnoughTicks = _velEnoughTicks;
        recognizeOutOfRange = _recognizeOutOfRange;
    }
 
    uint8_t getAutoGain()
    {
        return readRegister8(AGC_REG);
    }
 
    uint16_t readAngle()
    {
        return (rawReading() + STEPS_PER_TURN - zeroAngle) % STEPS_PER_TURN;
    }
 
    void useCustomWire(TwoWire& _wire)
    {
        wire = &_wire;
    }
 
    void run()
    {
        if (!(recognizeOutOfRange && !isMagnetInRange())) {
 
            angle = readAngle();
            if (abs((int16_t)angle - (int16_t)lastAngle) > STEPS_PER_TURN / 2) { // angle jump over half of circle is assumed to be the shorter crossing of 0
                if (angle > lastAngle) {
                    turns--;
                } else {
                    turns++;
                }
            }
 
            long velDist = ((int16_t)angle - (int16_t)lastVelAngle) + (turns - lastVelTurns) * STEPS_PER_TURN;
            if (micros() - lastVelTimeMicros > velEnoughTime || abs(velDist) > velEnoughTicks) {
                velocity = (double)1000000.0 * velDist / (micros() - lastVelTimeMicros) * distPerCountFactor * reverse;
                lastVelTimeMicros = micros();
                newSpeed = true;
                lastVelAngle = angle;
                lastVelTurns = turns;
            } else {
                newSpeed = false;
            }
 
            lastAngle = angle;
        } else {
            velocity = 0;
        }
    }
 
    void setEncoderZero(int _zeroAngle)
    {
        zeroAngle = constrain(_zeroAngle, 0, (long)STEPS_PER_TURN - 1);
    }
 
    void setEncoderZero()
    {
        setEncoderZero(rawReading());
    }
 
    int rawReading()
    {
        return readRegister12(RAWANGLEMSB_REG);
    }
 
    bool isMagnetInRange()
    {
        return (readRegister8(STATUS_REG) & 0b100000) > 0;
    }
 
    long intTurns()
    {
        return turns * reverse;
    }
 
    long zeroCounter()
    {
        return zeroCounter(true);
    }
 
    long zeroCounter(bool _resetAngle)
    {
        long tTurns = turns;
        turns = 0;
        if (_resetAngle) {
            setEncoderZero();
        }
        return (tTurns * STEPS_PER_TURN + angle) * reverse;
    }
 
    float getVel()
    {
        return velocity;
    }
 
    long getCounter()
    {
        return (turns * STEPS_PER_TURN + angle) * reverse;
    }
 
    float getPos()
    {
        return (int32_t)((turns * STEPS_PER_TURN + angle) * reverse) * distPerCountFactor;
    }
 
    float getDistPerCountFactor()
    {
        return distPerCountFactor;
    }
 
    void setDistPerCountFactor(float _factor)
    {
        distPerCountFactor = _factor / STEPS_PER_TURN;
    }
 
    bool hasDirection()
    {
        return true;
    }
 
    bool isVelNew()
    {
        return newSpeed;
    }
    void setVelEnoughTime(unsigned long _velEnoughTime)
    {
        velEnoughTime = _velEnoughTime;
    }
    void setVelEnoughTicks(unsigned long _velEnoughTicks)
    {
        velEnoughTicks = _velEnoughTicks;
    }
    void setRecognizeOutOfRange(bool _recognizeOutOfRange)
    {
        recognizeOutOfRange = _recognizeOutOfRange;
    }
};
#endif // JENCODER_AS5600I2C_H