diff --git a/src/encoders/ma735/MA735.cpp b/src/encoders/ma735/MA735.cpp new file mode 100644 index 0000000..fa5a803 --- /dev/null +++ b/src/encoders/ma735/MA735.cpp @@ -0,0 +1,297 @@ +#include "MA735.h" + +MA735::MA735(SPISettings settings, int nCS) : settings(settings), nCS(nCS) { + +}; +MA735::~MA735() { + +}; + +void MA735::init(SPIClass* _spi) { + spi = _spi; + if (nCS >= 0) { + pinMode(nCS, OUTPUT); + digitalWrite(nCS, HIGH); + } +}; + +float MA735::getCurrentAngle() { + return (readRawAngle() * _2PI)/MA735_16BIT;//It doesn't matter that it is divided by 65536, because the raw angle fills empty data bits with empty zeros so sensor resolution doesn't affect angle calculation +}; // angle in radians, return current value + +uint16_t MA735::readRawAngle() { + uint16_t angle = transfer16(0x0000); + return angle; +}; // 9-13bit angle value + +uint16_t MA735::getZero() { + uint16_t result = readRegister(MA735_REG_ZERO_POSITION_MSB)<<8; + result |= readRegister(MA735_REG_ZERO_POSITION_LSB); + return result; +}; +uint8_t MA735::getBiasCurrentTrimming() { + return readRegister(MA735_REG_BCT); +}; +bool MA735::isBiasCurrrentTrimmingX() { + return (readRegister(MA735_REG_ET) & 0x01)==0x01; +}; +bool MA735::isBiasCurrrentTrimmingY() { + return (readRegister(MA735_REG_ET) & 0x02)==0x02; +}; +uint16_t MA735::getPulsesPerTurn() { + uint16_t result = readRegister(MA735_REG_ILIP_PPT_LSB)>>6; + result |= ((uint16_t)readRegister(MA735_REG_PPT_MSB))<<2; + return result+1; +}; +uint8_t MA735::getIndexLength() { + return (readRegister(MA735_REG_ILIP_PPT_LSB)>>2)&0x0F; +}; +uint8_t MA735::getRotationDirection() { + return (readRegister(MA735_REG_RD)>>7); +}; +uint8_t MA735::getFieldStrengthHighThreshold() { + return (readRegister(MA735_REG_MGLT_MGHT)>>2)&0x07; +}; +uint8_t MA735::getFieldStrengthLowThreshold() { + return (readRegister(MA735_REG_MGLT_MGHT)>>5)&0x07; +}; +FieldStrength MA735::getFieldStrength() { + return (FieldStrength)(readRegister(MA735_REG_MGH_MGL)>>6); +}; +uint8_t MA735::getFilterWindow() { + return readRegister(MA735_REG_FW); +}; +uint8_t MA735::getHysteresis() { + return readRegister(MA735_REG_HYS); +}; +float MA735::getResolution() { + //All I could find in the datasheet was a table with the correlation, no function to convert Filter window to res. + uint8_t reg = readRegister(MA735_REG_FW); + float result; + switch (reg) { + case 51: + result = 9.0; + break; + case 68: + result = 9.5; + break; + case 85: + result = 10.0; + break; + case 102: + result = 10.5; + break; + case 119: + result = 11.0; + break; + case 136: + result = 11.5; + break; + case 153: + result = 12.0; + break; + case 170: + result = 12.5; + break; + case 187: + result = 13.0; + break; + default: + result = 11.0; + break; + } + return result; +}; +int MA735::getUpdateTime() { + //All I could find in the datasheet was a table with the correlation, no function to convert Filter window to update time. + //Returns result in microseconds + uint8_t reg = readRegister(MA735_REG_FW); + int result; + switch (reg) { + case 51: + result = 64; + break; + case 68: + result = 128; + break; + case 85: + result = 256; + break; + case 102: + result = 512; + break; + case 119: + result = 1024; + break; + case 136: + result = 2048; + break; + case 153: + result = 4096; + break; + case 170: + result = 8192; + break; + case 187: + result = 16384; + break; + default: + result = 1024; + break; + } + return result; +}; + + + +void MA735::setZero(uint16_t value) { + writeRegister(MA735_REG_ZERO_POSITION_MSB, value>>8); + writeRegister(MA735_REG_ZERO_POSITION_LSB, value&0x00FF); +}; +void MA735::setBiasCurrentTrimming(uint8_t value) { + writeRegister(MA735_REG_BCT, value); +}; +void MA735::setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled) { + uint8_t val = Xenabled ? 0x01 : 0x00; + val |= (Yenabled ? 0x02 : 0x00); + writeRegister(MA735_REG_ET, val); +}; +void MA735::setPulsesPerTurn(uint16_t value) { + uint16_t pptVal = value - 1; + writeRegister(MA735_REG_PPT_MSB, pptVal>>2); + uint8_t val = readRegister(MA735_REG_ILIP_PPT_LSB); + val &= 0x3F; + val |= (pptVal&0x03)<<6; + writeRegister(MA735_REG_ILIP_PPT_LSB, val); +}; +void MA735::setIndexLength(uint8_t value) { + uint8_t val = readRegister(MA735_REG_ILIP_PPT_LSB); + val &= 0xC0; + val |= ((value<<2)&0x3F); + writeRegister(MA735_REG_ILIP_PPT_LSB, val); +}; +void MA735::setRotationDirection(uint8_t value) { + if (value==0) + writeRegister(MA735_REG_RD, 0x00); + else + writeRegister(MA735_REG_RD, 0x80); +}; +void MA735::setFieldStrengthThresholds(uint8_t high, uint8_t low) { + uint8_t val = (low<<5) | (high<<2); + writeRegister(MA735_REG_MGLT_MGHT, val); +}; +void MA735::setFilterWindow(uint8_t value) { + writeRegister(MA735_REG_FW, value); +}; +void MA735::setHysteresis(uint8_t value) { + writeRegister(MA735_REG_HYS, value); +}; +void MA735::setResolution(float res) { + //All I could find in the datasheet was a table with the correlation, no function to convert Filter window to res. + uint8_t value; + uint8_t res_int = res * 10;//It has to be a basic type for the switch case + switch (res_int) { + case 90: + value = 51; + break; + case 95: + value = 68; + break; + case 100: + value = 85; + break; + case 105: + value = 102; + break; + case 110: + value = 119; + break; + case 115: + value = 136; + break; + case 120: + value = 153; + break; + case 125: + value = 170; + break; + case 130: + value = 187; + break; + default: + value = 119; + break; + } + writeRegister(MA735_REG_FW, value); +}; +void MA735::setUpdateTime(int microsec) { + //All I could find in the datasheet was a table with the correlation, no function to convert Filter window to update time. + //time in microseconds + uint8_t value; + switch (microsec) { + case 64: + value = 51; + break; + case 128: + value = 68; + break; + case 256: + value = 85; + break; + case 512: + value = 102; + break; + case 1024: + value = 119; + break; + case 2048: + value = 136; + break; + case 4096: + value = 153; + break; + case 8192: + value = 170; + break; + case 16384: + value = 187; + break; + default: + value = 119; + break; + } + writeRegister(MA735_REG_FW, value); +}; + + +uint16_t MA735::transfer16(uint16_t outValue) { + spi->beginTransaction(settings); + if (nCS >= 0) + digitalWrite(nCS, LOW); + uint16_t value = spi->transfer16(outValue); + if (nCS >= 0) + digitalWrite(nCS, HIGH); + spi->endTransaction(); + return value; +}; +uint8_t MA735::readRegister(uint8_t reg) { + uint16_t cmd = 0x4000 | ((reg&0x001F)<<8); + uint16_t value = transfer16(cmd); + delayMicroseconds(1); + value = transfer16(0x0000); + return value>>8; +}; +uint8_t MA735::writeRegister(uint8_t reg, uint8_t value) { + uint8_t write_check = readRegister(reg); + //no need to rewrite, it is the exact same value + if (write_check == value) { + return write_check; + } + else { + uint16_t cmd = 0x8000 | ((reg&0x1F)<<8) | value; + uint16_t result = transfer16(cmd); + delay(20); // 20ms delay required + result = transfer16(0x0000); + return result>>8; + } +}; diff --git a/src/encoders/ma735/MA735.h b/src/encoders/ma735/MA735.h new file mode 100644 index 0000000..77f07b1 --- /dev/null +++ b/src/encoders/ma735/MA735.h @@ -0,0 +1,87 @@ +#ifndef __MA735_H__ +#define __MA735_H__ + + +#include "Arduino.h" +#include "SPI.h" + +enum FieldStrength : uint8_t { + FS_NORMAL = 0x00, + FS_LOW = 0x01, + FS_HIGH = 0x02, + FS_ERR = 0x03 // impossible state +}; + + +#define _2PI 6.28318530718f +#define MA735_16BIT 65536.0f + +#define MA735_REG_ZERO_POSITION_LSB 0x00 +#define MA735_REG_ZERO_POSITION_MSB 0x01 +#define MA735_REG_BCT 0x02 +#define MA735_REG_ET 0x03 +#define MA735_REG_ILIP_PPT_LSB 0x04 +#define MA735_REG_PPT_MSB 0x05 +#define MA735_REG_MGLT_MGHT 0x06 +#define MA735_REG_RD 0x09 +#define MA735_REG_FW 0x0E +#define MA735_REG_HYS 0x10 +#define MA735_REG_MGH_MGL 0x1B + +#define MA735_BITORDER MSBFIRST + +static SPISettings MA735SPISettings(1000000, MA735_BITORDER, SPI_MODE3); // @suppress("Invalid arguments") +static SPISettings MA735SSISettings(4000000, MA735_BITORDER, SPI_MODE1); // @suppress("Invalid arguments") + + +class MA735 { +public: + MA735(SPISettings settings = MA735SPISettings, int nCS = -1); + virtual ~MA735(); + + virtual void init(SPIClass* _spi = &SPI); + + float getCurrentAngle(); // angle in radians, return current value + + uint16_t readRawAngle(); // 9-13bit angle value + uint16_t readRawAngleSSI(); // 9-13bit angle value + + uint16_t getZero(); + uint8_t getBiasCurrentTrimming(); + bool isBiasCurrrentTrimmingX(); + bool isBiasCurrrentTrimmingY(); + uint16_t getPulsesPerTurn(); + uint8_t getIndexLength(); + uint8_t getRotationDirection(); + uint8_t getFieldStrengthHighThreshold(); + uint8_t getFieldStrengthLowThreshold(); + FieldStrength getFieldStrength(); + uint8_t getFilterWindow(); + uint8_t getHysteresis(); + float getResolution(); + int getUpdateTime(); + + void setZero(uint16_t); + void setBiasCurrentTrimming(uint8_t); + void setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled); + void setPulsesPerTurn(uint16_t); + void setIndexLength(uint8_t); + void setRotationDirection(uint8_t); + void setFieldStrengthThresholds(uint8_t high, uint8_t low); + void setFilterWindow(uint8_t); + void setHysteresis(uint8_t); + void setResolution(float resolution); + void setUpdateTime(int microsec); + +private: + SPIClass* spi; + SPISettings settings; + int nCS = -1; + //float MA735_CPR = 65536.0f; + + uint16_t transfer16(uint16_t outValue); + uint8_t readRegister(uint8_t reg); + uint8_t writeRegister(uint8_t reg, uint8_t value); +}; + +#endif diff --git a/src/encoders/ma735/MagneticSensorMA735.cpp b/src/encoders/ma735/MagneticSensorMA735.cpp new file mode 100644 index 0000000..d16ecf4 --- /dev/null +++ b/src/encoders/ma735/MagneticSensorMA735.cpp @@ -0,0 +1,26 @@ +#include "./MagneticSensorMA735.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + +MagneticSensorMA735::MagneticSensorMA735(int nCS, SPISettings settings) : MA735(settings, nCS) { + +} + + +MagneticSensorMA735::~MagneticSensorMA735(){ + +} + + +void MagneticSensorMA735::init(SPIClass* _spi) { + this->MA735::init(_spi); + this->Sensor::init(); +} + + +float MagneticSensorMA735::getSensorAngle() { + float angle_data = readRawAngle(); + angle_data = ( angle_data / (float)MA735_16BIT) * _2PI;//It doesn't matter that it is divided by 65536, because the raw angle fills empty data bits with empty zeros so sensor resolution doesn't affect angle calculation + // return the shaft angle + return angle_data; +} diff --git a/src/encoders/ma735/MagneticSensorMA735.h b/src/encoders/ma735/MagneticSensorMA735.h new file mode 100644 index 0000000..36f697d --- /dev/null +++ b/src/encoders/ma735/MagneticSensorMA735.h @@ -0,0 +1,19 @@ +#ifndef __MAGNETIC_SENSOR_MA735_H__ +#define __MAGNETIC_SENSOR_MA735_H__ + + +#include "common/base_classes/Sensor.h" +#include "./MA735.h" + +class MagneticSensorMA735 : public Sensor, public MA735 { +public: + MagneticSensorMA735(int nCS = -1, SPISettings settings = MA735SPISettings); + virtual ~MagneticSensorMA735(); + + virtual float getSensorAngle() override; + + virtual void init(SPIClass* _spi = &SPI); +}; + + +#endif diff --git a/src/encoders/ma735/MagneticSensorMA735SSI.cpp b/src/encoders/ma735/MagneticSensorMA735SSI.cpp new file mode 100644 index 0000000..eaea82e --- /dev/null +++ b/src/encoders/ma735/MagneticSensorMA735SSI.cpp @@ -0,0 +1,34 @@ +#include "./MagneticSensorMA735SSI.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + +MagneticSensorMA735SSI::MagneticSensorMA735SSI(SPISettings settings) : settings(settings) { + +} + + +MagneticSensorMA735SSI::~MagneticSensorMA735SSI() { + +} + +void MagneticSensorMA735SSI::init(SPIClass* _spi) { + this->spi=_spi; + this->Sensor::init(); +} + +// check 40us delay between each read? +float MagneticSensorMA735SSI::getSensorAngle() { + float angle_data = readRawAngleSSI(); + angle_data = ( angle_data / (float)MA735_16BIT ) * _2PI;//It doesn't matter that it is divided by 65536, because the raw angle fills empty data bits with empty zeros so sensor resolution doesn't affect angle calculation + // return the shaft angle + return angle_data; +} + + +uint16_t MagneticSensorMA735SSI::readRawAngleSSI() { + spi->beginTransaction(settings); + uint16_t value = spi->transfer16(0x0000); + //uint16_t parity = spi->transfer(0x00); + spi->endTransaction(); + return (value<<1); //>>1)&0x3FFF; +}; // 9-13bit angle value diff --git a/src/encoders/ma735/MagneticSensorMA735SSI.h b/src/encoders/ma735/MagneticSensorMA735SSI.h new file mode 100644 index 0000000..37df636 --- /dev/null +++ b/src/encoders/ma735/MagneticSensorMA735SSI.h @@ -0,0 +1,25 @@ +#ifndef __MAGNETIC_SENSOR_MA735SSI_H__ +#define __MAGNETIC_SENSOR_MA735SSI_H__ + + +#include "common/base_classes/Sensor.h" +#include "./MA735.h" + +class MagneticSensorMA735SSI : public Sensor { +public: + MagneticSensorMA735SSI(SPISettings settings = MA735SSISettings); + virtual ~MagneticSensorMA735SSI(); + + virtual float getSensorAngle() override; + + virtual void init(SPIClass* _spi = &SPI); + + uint16_t readRawAngleSSI(); + +private: + SPIClass* spi; + SPISettings settings; +}; + + +#endif diff --git a/src/encoders/ma735/README.md b/src/encoders/ma735/README.md new file mode 100644 index 0000000..eb5f10d --- /dev/null +++ b/src/encoders/ma735/README.md @@ -0,0 +1,84 @@ +# MA735 SimpleFOC driver + +This is based on the [MA730](https://github.com/simplefoc/Arduino-FOC-drivers/tree/master/src/encoders/ma730) driver in SimpleFOC, with some tweaks to support the unique registers and options of the MA735. It is a little bit inferior, but less expensive chip after all. The advantage however of this chip (besides cost) is that resolution is inversely proportional to reading speed, ranging from 9-13 bit resolution and 64us to 16ms read times. + + +## Hardware setup + +Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver. + +## Software setup + +Its actually easier to use than the standard SPI sensor class, because it is less generic: + +```c++ +#include "Arduino.h" +#include "Wire.h" +#include "SPI.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "encoders/ma735/MagneticSensorMA735.h" + +#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin +MagneticSensorMA735 sensor1(SENSOR1_CS); + + +void setup() { + sensor1.init(); +} +``` + +Set some options: + +```c++ +MagneticSensorMA735 sensor1(SENSOR1_CS, mySPISettings); +``` + +Use another SPI bus: + +```c++ +void setup() { + sensor1.init(&SPI2); +} +``` + +Set some registers before start +```c++ +void setup() { + sensor1.init(); + + // Note that with this driver there is a write check so registers are not written to if the value is the exact same. Other drivers do not have a write check, and you can easily wear out the NVM every time the code is run. 1,000 cycles max. + sensor1.setResolution(10.0); +} +``` + +Here's how you can use it: + +```c++ + // update the sensor (only needed if using the sensor without a motor) + sensor1.update(); + + // get the angle, in radians, including full rotations + float a1 = sensor1.getAngle(); + + // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work + float v1 = sensor1.getVelocity(); + + // get the angle, in radians, no full rotations + float a2 = sensor1.getCurrentAngle(); + + // get the field strength + FieldStrength fs = sensor1.getFieldStrength(); + Serial.print("Field strength: "); + Serial.println(fs); + + //get the calculated resolution from FilterWindow register + float res = sensor.getResolution(); + Serial1.print("Resolution: "); + Serial1.println(res); + + //get the calculated update time in micro seconds from the FilterWindow register + int Time = sensor.getUpdateTime(); + Serial1.print("Update Time (microsecs): "); + Serial1.println(Time); +```