From 07fbcc9b1b227d581862a6768a60f338f16e279f Mon Sep 17 00:00:00 2001 From: techy-robot Date: Wed, 17 Jul 2024 11:50:30 -0600 Subject: [PATCH 1/2] Added support for MA735 This is largely based off of the code for MA730, with some additional functions to support the variable resolution and speed. The MA735 is probably one of the cheapest Mag Alpha sensors out there. --- src/encoders/ma735/MA735.cpp | 297 ++++++++++++++++++ src/encoders/ma735/MA735.h | 87 +++++ src/encoders/ma735/MagneticSensorMA735.cpp | 26 ++ src/encoders/ma735/MagneticSensorMA735.h | 19 ++ src/encoders/ma735/MagneticSensorMA735SSI.cpp | 34 ++ src/encoders/ma735/MagneticSensorMA735SSI.h | 25 ++ src/encoders/ma735/README.md | 70 +++++ 7 files changed, 558 insertions(+) create mode 100644 src/encoders/ma735/MA735.cpp create mode 100644 src/encoders/ma735/MA735.h create mode 100644 src/encoders/ma735/MagneticSensorMA735.cpp create mode 100644 src/encoders/ma735/MagneticSensorMA735.h create mode 100644 src/encoders/ma735/MagneticSensorMA735SSI.cpp create mode 100644 src/encoders/ma735/MagneticSensorMA735SSI.h create mode 100644 src/encoders/ma735/README.md 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..d1b402a --- /dev/null +++ b/src/encoders/ma735/README.md @@ -0,0 +1,70 @@ +# 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 inferior, but less expensive chip after all. + + +## 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); +} +``` + +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 raw 14 bit value + uint16_t raw = sensor1.readRawAngle(); + + // get the field strength + FieldStrength fs = sensor1.getFieldStrength(); + Serial.print("Field strength: "); + Serial.println(fs); + + // set pulses per turn for encoder mode + sensor1.setPulsesPerTurn(999); // set to 999 if we want 1000 PPR == 4000 CPR +``` From 9b895ce8adfcf303e31ab790d2a798082e77a72f Mon Sep 17 00:00:00 2001 From: techy-robot Date: Wed, 17 Jul 2024 12:19:23 -0600 Subject: [PATCH 2/2] Updates README file examples of of use --- src/encoders/ma735/README.md | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/encoders/ma735/README.md b/src/encoders/ma735/README.md index d1b402a..eb5f10d 100644 --- a/src/encoders/ma735/README.md +++ b/src/encoders/ma735/README.md @@ -1,6 +1,6 @@ # 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 inferior, but less expensive chip after all. +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 @@ -42,6 +42,16 @@ void setup() { } ``` +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++ @@ -57,14 +67,18 @@ Here's how you can use it: // get the angle, in radians, no full rotations float a2 = sensor1.getCurrentAngle(); - // get the raw 14 bit value - uint16_t raw = sensor1.readRawAngle(); - // get the field strength FieldStrength fs = sensor1.getFieldStrength(); Serial.print("Field strength: "); Serial.println(fs); - // set pulses per turn for encoder mode - sensor1.setPulsesPerTurn(999); // set to 999 if we want 1000 PPR == 4000 CPR + //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); ```