WipperSnapper_I2C_Driver_AS5600.h

Go to the documentation of this file.

#ifndef WipperSnapper_I2C_Driver_AS5600_H
#define WipperSnapper_I2C_Driver_AS5600_H

#include <Adafruit_AS5600.h>

#include "WipperSnapper_I2C_Driver.h"
#include "Wippersnapper.h"

/**************************************************************************/
/**************************************************************************/
class WipperSnapper_I2C_Driver_AS5600 : public WipperSnapper_I2C_Driver {
public:
  /*******************************************************************************/
  /*******************************************************************************/
  WipperSnapper_I2C_Driver_AS5600(TwoWire *i2c, uint16_t sensorAddress)
      : WipperSnapper_I2C_Driver(i2c, sensorAddress) {
    _as5600 = nullptr;
    _angle = 0;
  }

  /*******************************************************************************/
  /*******************************************************************************/
  ~WipperSnapper_I2C_Driver_AS5600() {
    if (_as5600) {
      delete _as5600;
      _as5600 = nullptr;
    }
  }

  /*******************************************************************************/
  /*******************************************************************************/
  bool begin() {
    _as5600 = new Adafruit_AS5600();
    if (!_as5600->begin((uint8_t)_sensorAddress, _i2c)) {
      WS_DEBUG_PRINTLN("Failed to find AS5600 chip");
      return false;
    }

    if (!configureSensor()) {
      WS_DEBUG_PRINTLN("Failed to configure AS5600 sensor");
      return false;
    }
    return true;
  }

  /*******************************************************************************/
  /*******************************************************************************/
  bool configureSensor() {
    return _as5600->enableWatchdog(false) &&
           // Normal (high) power mode
           _as5600->setPowerMode(AS5600_POWER_MODE_NOM) &&
           // No Hysteresis
           _as5600->setHysteresis(AS5600_HYSTERESIS_OFF) &&
           // analog output (0-VCC for 0-360 degrees)
           _as5600->setOutputStage(AS5600_OUTPUT_STAGE_ANALOG_FULL) &&
           // setup filters
           _as5600->setSlowFilter(AS5600_SLOW_FILTER_16X) &&
           _as5600->setFastFilterThresh(AS5600_FAST_FILTER_THRESH_SLOW_ONLY) &&
           // Reset position settings to defaults
           _as5600->setZPosition(0) && _as5600->setMPosition(4095) &&
           _as5600->setMaxAngle(4095);
  }

  /*******************************************************************************/
  /*******************************************************************************/
  bool readSensor() {
    if (!_as5600->isMagnetDetected()) {
      WS_DEBUG_PRINTLN("Magnet not detected!");
      return false;
    }

    // Continuously read and display angle values
    uint16_t rawAngle = _as5600->getRawAngle();
    uint16_t angle = _as5600->getAngle();

    WS_DEBUG_PRINT("AS5600 Raw: ");
    WS_DEBUG_PRINT(rawAngle);
    WS_DEBUG_PRINT(" | Scaled: ");
    WS_DEBUG_PRINT(angle);

    // Check status conditions
    if (_as5600->isAGCminGainOverflow()) {
      WS_DEBUG_PRINTLN(" | MH: magnet too strong");
      return false;
    }
    if (_as5600->isAGCmaxGainOverflow()) {
      WS_DEBUG_PRINTLN(" | ML: magnet too weak");
      return false;
    }
    _angle = ((float)angle / 4095.0) * 360.0;
    return true;
  }

  /*******************************************************************************/
  /*******************************************************************************/
  bool getEventRaw(sensors_event_t *rawEvent) {
    if (!readSensor()) {
      return false;
    }
    rawEvent->data[0] = _angle;
    return true;
  }

protected:
  float _angle;             
  Adafruit_AS5600 *_as5600; 
};

#endif // WipperSnapper_I2C_Driver_AS5600