mirror of
https://github.com/RobTillaart/Arduino.git
synced 2024-10-03 18:09:02 -04:00
506 lines
11 KiB
C++
506 lines
11 KiB
C++
//
|
|
// FILE: AS56000.cpp
|
|
// AUTHOR: Rob Tillaart
|
|
// VERSION: 0.3.0
|
|
// PURPOSE: Arduino library for AS5600 magnetic rotation meter
|
|
// DATE: 2022-05-28
|
|
// URL: https://github.com/RobTillaart/AS5600
|
|
|
|
// 0.1.0 2022-05-28 initial version.
|
|
// 0.1.1 2022-05-31 Add readReg2() to speed up reading 2 byte values.
|
|
// Fix clock wise and counter clock wise.
|
|
// Fix shift-direction @ getZPosition, getMPosition,
|
|
// getMaxAngle and getConfigure.
|
|
// 0.1.2 2022-06-02 Add getAngularSpeed().
|
|
// 0.1.3 2022-06-26 Add AS5600_RAW_TO_RADIANS.
|
|
// Add getAngularSpeed() mode parameter.
|
|
// Fix #8 bug in configure.
|
|
// 0.1.4 2022-06-27 Fix #7 use readReg2() to improve I2C performance.
|
|
// define constants for configuration functions.
|
|
// add examples - especially OUT pin related.
|
|
// Fix default parameter of the begin function.
|
|
//
|
|
// 0.2.0 2022-06-28 add software based direction control.
|
|
// add examples
|
|
// define constants for configuration functions.
|
|
// fix conversion constants (4096 based)
|
|
// add get- setOffset(degrees) functions. (no radians yet)
|
|
// 0.2.1 notreleased add bool return to set() functions.
|
|
// update Readme (analog / PWM out)
|
|
// 0.3.0 2022-07-07 fix #18 invalid mask setConfigure().
|
|
|
|
|
|
// TODO
|
|
// Power-up time 1 minute (need HW)
|
|
// check Timing Characteristics
|
|
|
|
|
|
#include "AS5600.h"
|
|
|
|
|
|
// CONFIGURATION REGISTERS
|
|
const uint8_t AS5600_ZMCO = 0x00;
|
|
const uint8_t AS5600_ZPOS = 0x01; // + 0x02
|
|
const uint8_t AS5600_MPOS = 0x03; // + 0x04
|
|
const uint8_t AS5600_MANG = 0x05; // + 0x06
|
|
const uint8_t AS5600_CONF = 0x07; // + 0x08
|
|
|
|
// CONFIGURATION BIT MASKS - byte level
|
|
const uint8_t AS5600_CONF_POWER_MODE = 0x03;
|
|
const uint8_t AS5600_CONF_HYSTERESIS = 0x0C;
|
|
const uint8_t AS5600_CONF_OUTPUT_MODE = 0x30;
|
|
const uint8_t AS5600_CONF_PWM_FREQUENCY = 0xC0;
|
|
const uint8_t AS5600_CONF_SLOW_FILTER = 0x03;
|
|
const uint8_t AS5600_CONF_FAST_FILTER = 0x1C;
|
|
const uint8_t AS5600_CONF_WATCH_DOG = 0x20;
|
|
|
|
|
|
// UNKNOWN REGISTERS 0x09-0x0A
|
|
|
|
// OUTPUT REGISTERS
|
|
const uint8_t AS5600_RAW_ANGLE = 0x0C; // + 0x0D
|
|
const uint8_t AS5600_ANGLE = 0x0E; // + 0x0F
|
|
|
|
// STATUS REGISTERS
|
|
const uint8_t AS5600_STATUS = 0x0B;
|
|
const uint8_t AS5600_AGC = 0x1A;
|
|
const uint8_t AS5600_MAGNITUDE = 0x1B; // + 0x1C
|
|
const uint8_t AS5600_BURN = 0xFF;
|
|
|
|
// STATUS BITS
|
|
const uint8_t AS5600_MAGNET_HIGH = 0x08;
|
|
const uint8_t AS5600_MAGNET_LOW = 0x10;
|
|
const uint8_t AS5600_MAGNET_DETECT = 0x20;
|
|
|
|
|
|
AS5600::AS5600(TwoWire *wire)
|
|
{
|
|
_wire = wire;
|
|
}
|
|
|
|
|
|
#if defined (ESP8266) || defined(ESP32)
|
|
bool AS5600::begin(int dataPin, int clockPin, uint8_t directionPin)
|
|
{
|
|
_directionPin = directionPin;
|
|
if (_directionPin != 255)
|
|
{
|
|
pinMode(_directionPin, OUTPUT);
|
|
}
|
|
setDirection(AS5600_CLOCK_WISE);
|
|
|
|
_wire = &Wire;
|
|
if ((dataPin < 255) && (clockPin < 255))
|
|
{
|
|
_wire->begin(dataPin, clockPin);
|
|
} else {
|
|
_wire->begin();
|
|
}
|
|
if (! isConnected()) return false;
|
|
return true;
|
|
}
|
|
#endif
|
|
|
|
|
|
bool AS5600::begin(uint8_t directionPin)
|
|
{
|
|
_directionPin = directionPin;
|
|
if (_directionPin != 255)
|
|
{
|
|
pinMode(_directionPin, OUTPUT);
|
|
}
|
|
setDirection(AS5600_CLOCK_WISE);
|
|
|
|
_wire->begin();
|
|
if (! isConnected()) return false;
|
|
return true;
|
|
}
|
|
|
|
|
|
bool AS5600::isConnected()
|
|
{
|
|
_wire->beginTransmission(_address);
|
|
return ( _wire->endTransmission() == 0);
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////
|
|
//
|
|
// CONFIGURATION REGISTERS + direction pin
|
|
//
|
|
void AS5600::setDirection(uint8_t direction)
|
|
{
|
|
_direction = direction;
|
|
if (_directionPin != 255)
|
|
{
|
|
digitalWrite(_directionPin, _direction);
|
|
}
|
|
}
|
|
|
|
|
|
uint8_t AS5600::getDirection()
|
|
{
|
|
if (_directionPin != 255)
|
|
{
|
|
_direction = digitalRead(_directionPin);
|
|
}
|
|
return _direction;
|
|
}
|
|
|
|
|
|
uint8_t AS5600::getZMCO()
|
|
{
|
|
uint8_t value = readReg(AS5600_ZMCO);
|
|
return value;
|
|
}
|
|
|
|
|
|
bool AS5600::setZPosition(uint16_t value)
|
|
{
|
|
if (value > 0x0FFF) return false;
|
|
writeReg2(AS5600_ZPOS, value);
|
|
return true;
|
|
}
|
|
|
|
|
|
uint16_t AS5600::getZPosition()
|
|
{
|
|
uint16_t value = readReg2(AS5600_ZPOS) & 0x0FFF;
|
|
return value;
|
|
}
|
|
|
|
|
|
bool AS5600::setMPosition(uint16_t value)
|
|
{
|
|
if (value > 0x0FFF) return false;
|
|
writeReg2(AS5600_MPOS, value);
|
|
return true;
|
|
}
|
|
|
|
|
|
uint16_t AS5600::getMPosition()
|
|
{
|
|
uint16_t value = readReg2(AS5600_MPOS) & 0x0FFF;
|
|
return value;
|
|
}
|
|
|
|
|
|
bool AS5600::setMaxAngle(uint16_t value)
|
|
{
|
|
if (value > 0x0FFF) return false;
|
|
writeReg2(AS5600_MANG, value);
|
|
return true;
|
|
}
|
|
|
|
|
|
uint16_t AS5600::getMaxAngle()
|
|
{
|
|
uint16_t value = readReg2(AS5600_MANG) & 0x0FFF;
|
|
return value;
|
|
}
|
|
|
|
|
|
bool AS5600::setConfigure(uint16_t value)
|
|
{
|
|
if (value > 0x3FFF) return false;
|
|
writeReg2(AS5600_CONF, value);
|
|
return true;
|
|
}
|
|
|
|
|
|
uint16_t AS5600::getConfigure()
|
|
{
|
|
uint16_t value = readReg2(AS5600_CONF) & 0x3FFF;
|
|
return value;
|
|
}
|
|
|
|
|
|
// details configure
|
|
bool AS5600::setPowerMode(uint8_t powerMode)
|
|
{
|
|
if (powerMode > 3) return false;
|
|
uint8_t value = readReg(AS5600_CONF + 1);
|
|
value &= ~AS5600_CONF_POWER_MODE;
|
|
value |= powerMode;
|
|
writeReg(AS5600_CONF + 1, value);
|
|
return true;
|
|
}
|
|
|
|
uint8_t AS5600::getPowerMode()
|
|
{
|
|
return readReg(AS5600_CONF + 1) & 0x03;
|
|
}
|
|
|
|
bool AS5600::setHysteresis(uint8_t hysteresis)
|
|
{
|
|
if (hysteresis > 3) return false;
|
|
uint8_t value = readReg(AS5600_CONF + 1);
|
|
value &= ~AS5600_CONF_HYSTERESIS;
|
|
value |= (hysteresis << 2);
|
|
writeReg(AS5600_CONF + 1, value);
|
|
return true;
|
|
}
|
|
|
|
uint8_t AS5600::getHysteresis()
|
|
{
|
|
return (readReg(AS5600_CONF + 1) >> 2) & 0x03;
|
|
}
|
|
|
|
bool AS5600::setOutputMode(uint8_t outputMode)
|
|
{
|
|
if (outputMode > 2) return false;
|
|
uint8_t value = readReg(AS5600_CONF + 1);
|
|
value &= ~AS5600_CONF_OUTPUT_MODE;
|
|
value |= (outputMode << 4);
|
|
writeReg(AS5600_CONF + 1, value);
|
|
return true;
|
|
}
|
|
|
|
uint8_t AS5600::getOutputMode()
|
|
{
|
|
return (readReg(AS5600_CONF + 1) >> 4) & 0x03;
|
|
}
|
|
|
|
bool AS5600::setPWMFrequency(uint8_t pwmFreq)
|
|
{
|
|
if (pwmFreq > 3) return false;
|
|
uint8_t value = readReg(AS5600_CONF + 1);
|
|
value &= ~AS5600_CONF_PWM_FREQUENCY;
|
|
value |= (pwmFreq << 6);
|
|
writeReg(AS5600_CONF + 1, value);
|
|
return true;
|
|
}
|
|
|
|
uint8_t AS5600::getPWMFrequency()
|
|
{
|
|
return (readReg(AS5600_CONF + 1) >> 6) & 0x03;
|
|
}
|
|
|
|
bool AS5600::setSlowFilter(uint8_t mask)
|
|
{
|
|
if (mask > 3) return false;
|
|
uint8_t value = readReg(AS5600_CONF);
|
|
value &= ~AS5600_CONF_SLOW_FILTER;
|
|
value |= mask;
|
|
writeReg(AS5600_CONF, value);
|
|
return true;
|
|
}
|
|
|
|
uint8_t AS5600::getSlowFilter()
|
|
{
|
|
return readReg(AS5600_CONF) & 0x03;
|
|
}
|
|
|
|
bool AS5600::setFastFilter(uint8_t mask)
|
|
{
|
|
if (mask > 7) return false;
|
|
uint8_t value = readReg(AS5600_CONF);
|
|
value &= ~AS5600_CONF_FAST_FILTER;
|
|
value |= (mask << 2);
|
|
writeReg(AS5600_CONF, value);
|
|
return true;
|
|
}
|
|
|
|
uint8_t AS5600::getFastFilter()
|
|
{
|
|
return (readReg(AS5600_CONF) >> 2) & 0x07;
|
|
}
|
|
|
|
bool AS5600::setWatchDog(uint8_t mask)
|
|
{
|
|
if (mask > 1) return false;
|
|
uint8_t value = readReg(AS5600_CONF);
|
|
value &= ~AS5600_CONF_WATCH_DOG;
|
|
value |= (mask << 5);
|
|
writeReg(AS5600_CONF, value);
|
|
return true;
|
|
}
|
|
|
|
uint8_t AS5600::getWatchDog()
|
|
{
|
|
return (readReg(AS5600_CONF) >> 5) & 0x01;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////
|
|
//
|
|
// OUTPUT REGISTERS
|
|
//
|
|
uint16_t AS5600::rawAngle()
|
|
{
|
|
uint16_t value = readReg2(AS5600_RAW_ANGLE) & 0x0FFF;
|
|
if (_offset > 0) value = (value + _offset) & 0x0FFF;
|
|
|
|
if ((_directionPin == 255) && (_direction == AS5600_COUNTERCLOCK_WISE))
|
|
{
|
|
value = (4096 - value) & 4095;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
|
|
uint16_t AS5600::readAngle()
|
|
{
|
|
uint16_t value = readReg2(AS5600_ANGLE) & 0x0FFF;
|
|
if (_offset > 0) value = (value + _offset) & 0x0FFF;
|
|
|
|
if ((_directionPin == 255) && (_direction == AS5600_COUNTERCLOCK_WISE))
|
|
{
|
|
value = (4096 - value) & 4095;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
|
|
bool AS5600::setOffset(float degrees)
|
|
{
|
|
// expect loss of precision.
|
|
if (abs(degrees) > 36000) return false;
|
|
bool neg = (degrees < 0);
|
|
if (neg) degrees = -degrees;
|
|
|
|
uint16_t offset = round(degrees * (4096 / 360.0));
|
|
offset &= 4095;
|
|
if (neg) offset = 4096 - offset;
|
|
_offset = offset;
|
|
return true;
|
|
}
|
|
|
|
|
|
float AS5600::getOffset()
|
|
{
|
|
return _offset * AS5600_RAW_TO_DEGREES;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////
|
|
//
|
|
// STATUS REGISTERS
|
|
//
|
|
uint8_t AS5600::readStatus()
|
|
{
|
|
uint8_t value = readReg(AS5600_STATUS);
|
|
return value;
|
|
}
|
|
|
|
|
|
uint8_t AS5600::readAGC()
|
|
{
|
|
uint8_t value = readReg(AS5600_AGC);
|
|
return value;
|
|
}
|
|
|
|
|
|
uint16_t AS5600::readMagnitude()
|
|
{
|
|
uint16_t value = readReg2(AS5600_MAGNITUDE) & 0x0FFF;
|
|
return value;
|
|
}
|
|
|
|
|
|
bool AS5600::detectMagnet()
|
|
{
|
|
return (readStatus() & AS5600_MAGNET_DETECT) > 1;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////
|
|
//
|
|
// BURN COMMANDS
|
|
//
|
|
// DO NOT UNCOMMENT - USE AT OWN RISK - READ DATASHEET
|
|
//
|
|
// void AS5600::burnAngle()
|
|
// {
|
|
// writeReg(AS5600_BURN, x0x80);
|
|
// }
|
|
//
|
|
//
|
|
// void AS5600::burnSetting()
|
|
// {
|
|
// writeReg(AS5600_BURN, x0x40);
|
|
// }
|
|
|
|
|
|
float AS5600::getAngularSpeed(uint8_t mode)
|
|
{
|
|
uint32_t now = micros();
|
|
int angle = readAngle();
|
|
uint32_t deltaT = now - _lastMeasurement;
|
|
int deltaA = angle - _lastAngle;
|
|
|
|
// assumption is that there is no more than 180° rotation
|
|
// between two consecutive measurements.
|
|
// => at least two measurements per rotation (preferred 4).
|
|
if (deltaA > 2048) deltaA -= 4096;
|
|
if (deltaA < -2048) deltaA += 4096;
|
|
float speed = (deltaA * 1e6) / deltaT;
|
|
|
|
// remember last time & angle
|
|
_lastMeasurement = now;
|
|
_lastAngle = angle;
|
|
// return degrees or radians
|
|
if (mode == AS5600_MODE_RADIANS)
|
|
{
|
|
return speed * AS5600_RAW_TO_RADIANS;
|
|
}
|
|
// default return degrees
|
|
return speed * AS5600_RAW_TO_DEGREES;
|
|
}
|
|
|
|
|
|
/////////////////////////////////////////////////////////
|
|
//
|
|
// PRIVATE
|
|
//
|
|
uint8_t AS5600::readReg(uint8_t reg)
|
|
{
|
|
_wire->beginTransmission(_address);
|
|
_wire->write(reg);
|
|
_error = _wire->endTransmission();
|
|
|
|
_wire->requestFrom(_address, (uint8_t)1);
|
|
uint8_t _data = _wire->read();
|
|
return _data;
|
|
}
|
|
|
|
|
|
uint16_t AS5600::readReg2(uint8_t reg)
|
|
{
|
|
_wire->beginTransmission(_address);
|
|
_wire->write(reg);
|
|
_error = _wire->endTransmission();
|
|
|
|
_wire->requestFrom(_address, (uint8_t)2);
|
|
uint16_t _data = _wire->read();
|
|
_data <<= 8;
|
|
_data += _wire->read();
|
|
return _data;
|
|
}
|
|
|
|
|
|
uint8_t AS5600::writeReg(uint8_t reg, uint8_t value)
|
|
{
|
|
_wire->beginTransmission(_address);
|
|
_wire->write(reg);
|
|
_wire->write(value);
|
|
_error = _wire->endTransmission();
|
|
return _error;
|
|
}
|
|
|
|
|
|
uint8_t AS5600::writeReg2(uint8_t reg, uint16_t value)
|
|
{
|
|
_wire->beginTransmission(_address);
|
|
_wire->write(reg);
|
|
_wire->write(value >> 8);
|
|
_wire->write(value & 0xFF);
|
|
_error = _wire->endTransmission();
|
|
return _error;
|
|
}
|
|
|
|
|
|
// -- END OF FILE --
|
|
|
|
|