diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 4370526..162be4d 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -21,26 +21,26 @@ jobs: - adafruit:samd:adafruit_metro_m4 # samd51 - esp32:esp32:esp32 # esp32 - esp32:esp32:esp32s2 # esp32s2 - - STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - - STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - arduino:mbed_rp2040:pico # rpi pico include: - arduino-boards-fqbn: arduino:avr:uno - sketch-names: '**.ino' + sketches-exclude: calibrated mt6816_spi required-libraries: Simple FOC - arduino-boards-fqbn: arduino:sam:arduino_due_x required-libraries: Simple FOC - sketch-names: '**.ino' + sketches-exclude: calibrated - arduino-boards-fqbn: arduino:samd:nano_33_iot - required-libraries: Simple FOC - sketch-names: '**.ino' + required-libraries: Simple FOC + sketches-exclude: calibrated - arduino-boards-fqbn: arduino:mbed_rp2040:pico required-libraries: Simple FOC - sketch-names: '**.ino' + sketches-exclude: calibrated - arduino-boards-fqbn: adafruit:samd:adafruit_metro_m4 platform-url: https://adafruit.github.io/arduino-board-index/package_adafruit_index.json required-libraries: Simple FOC - sketch-names: '**.ino' + sketches-exclude: calibrated # - arduino-boards-fqbn: esp32:esp32:esp32doit-devkit-v1 # platform-url: https://dl.espressif.com/dl/package_esp32_index.json # required-libraries: Simple FOC @@ -48,19 +48,18 @@ jobs: - arduino-boards-fqbn: esp32:esp32:esp32 # esp32 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json required-libraries: Simple FOC - sketch-names: '**.ino' + sketches-exclude: calibrated - arduino-boards-fqbn: esp32:esp32:esp32s2 # esp32s2 platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json required-libraries: Simple FOC - sketch-names: '**.ino' - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + sketches-exclude: calibrated + - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC - sketch-names: '**.ino' - - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + sketches-exclude: calibrated mt6816_spi + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC - sketch-names: '**.ino' # Do not cancel all jobs / architectures if one job fails fail-fast: false steps: diff --git a/.gitignore b/.gitignore index 3a4edf6..ce7f6d0 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ .project +.DS_Store diff --git a/README.md b/README.md index 46f7983..1e40cc4 100644 --- a/README.md +++ b/README.md @@ -2,15 +2,25 @@ ![Library Compile](https://github.com/simplefoc/Arduino-FOC-drivers/workflows/Library%20Compile/badge.svg) [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) - +![Release](https://img.shields.io/badge/version-1.0.2-blue) This library contains an assortment of drivers and supporting code for SimpleFOC. The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, understand and port to different platforms. In addition to this core, there are various drivers and supporting code which has grown around SimpleFOC, and which we would like to make available to the community. -## What's contained +## New Release + +v1.0.2 - Released Oct 2022, for Simple FOC 2.2.3 + +What's changed since 1.0.1? +- Calibrated sensor by @MarethyuPrefect +- New Sensors: MT6701, MA330, MT6816 +- Fixes bugs + + +## What is included -What's here? See the sections below. Each driver or function should come with its own more detailed README. +What is here? See the sections below. Each driver or function should come with its own more detailed README. ### Motor/Gate driver ICs @@ -27,6 +37,9 @@ What's here? See the sections below. Each driver or function should come with it - [TLE5012B SPI driver](src/encoders/tle5012b/) - SPI (half duplex) driver for TLE5012B absolute position magnetic rotary encoder IC. - [STM32 Hardware Encoder](src/encoders/stm32hwencoder/) - Hardware timer based encoder driver for ABI type quadrature encoders. - [SC60228 SPI driver](src/encoders/sc60228/) - SPI driver for SemiMent SC60288 magnetic encoder IC. + - [MA330 SPI driver](src/encoders/ma330/) - SPI driver for the MPS MagAlpha MA330 absolute position magnetic rotary encoder IC. + - [MT6816 SPI driver](src/encoders/mt6816/) - SPI driver for the MagnTek MT6816 absolute position magnetic rotary encoder IC. + - [MT6701 SSI driver](src/encoders/mt6701/) - SSI driver for the MagnTek MT6701 absolute position magnetic rotary encoder IC. ### Communications diff --git a/examples/encoders/calibrated/calibrated.ino b/examples/encoders/calibrated/calibrated.ino new file mode 100644 index 0000000..6cc6110 --- /dev/null +++ b/examples/encoders/calibrated/calibrated.ino @@ -0,0 +1,86 @@ +/** + * Torque control example using voltage control loop. + * + * Most of the low-end BLDC driver boards doesn't have current measurement therefore SimpleFOC offers + * you a way to control motor torque by setting the voltage to the motor instead hte current. + * + * This makes the BLDC motor effectively a DC motor, and you can use it in a same way. + */ +#include +#include +#include "encoders/calibrated/CalibratedSensor.h" + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, PB6); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9); +// instantiate the calibrated sensor object +CalibratedSensor sensor_calibrated = CalibratedSensor(sensor); + +// voltage set point variable +float target_voltage = 2; + +// instantiate the commander +Commander command = Commander(Serial); + +void doTarget(char* cmd) { command.scalar(&target_voltage, cmd); } + +void setup() { + + SPI.setMISO(PB14); + SPI.setMOSI(PB15); + SPI.setSCLK(PB13); + + sensor.init(); + // Link motor to sensor + motor.linkSensor(&sensor); + // power supply voltage + driver.voltage_power_supply = 20; + driver.init(); + motor.linkDriver(&driver); + // aligning voltage + motor.voltage_sensor_align = 8; + motor.voltage_limit = 20; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_VEL; + motor.monitor_downsample = 10; // default 10 + + // initialize motor + motor.init(); + + // set voltage to run calibration + sensor_calibrated.voltage_calibration = 6; + // Running calibration + sensor_calibrated.calibrate(motor); + + //Serial.println("Calibrating Sensor Done."); + // Linking sensor to motor object + motor.linkSensor(&sensor_calibrated); + + // calibrated init FOC + motor.initFOC(); + + // add target command T + command.add('T', doTarget, "target voltage"); + + Serial.println(F("Motor ready.")); + + Serial.println(F("Set the target voltage using serial terminal:")); + _delay(1000); +} + +void loop() { + + motor.loopFOC(); + motor.move(target_voltage); + command.run(); + motor.monitor(); + +} \ No newline at end of file diff --git a/examples/encoders/mt6816/mt6816_spi/mt6816_spi.ino b/examples/encoders/mt6816/mt6816_spi/mt6816_spi.ino new file mode 100644 index 0000000..c1b49a3 --- /dev/null +++ b/examples/encoders/mt6816/mt6816_spi/mt6816_spi.ino @@ -0,0 +1,106 @@ +/** + * Comprehensive BLDC motor control example using magnetic sensor MT6816 + * + * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: + * - configure PID controller constants + * - change motion control loops + * - monitor motor variabels + * - set target values + * - check all the configuration values + * + * See more info in docs.simplefoc.com/commander_interface + */ +#include +#include +#include + +// magnetic sensor instance - MT6816 SPI mode +MagneticSensorMT6816 sensor = MagneticSensorMT6816(5); + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(32, 25, 26, 33); + +// Inline Current Sense instance +InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 35, 34); + +// commander interface +Commander command = Commander(Serial); +void onMotor(char* cmd){ command.motor(&motor, cmd); } + +void setup() { + + // initialise magnetic sensor hardware + sensor.init(); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 12; + driver.init(); + // link driver + motor.linkDriver(&driver); + + // choose FOC modulation + motor.foc_modulation = FOCModulationType::SpaceVectorPWM; + + // set control loop type to be used + motor.controller = MotionControlType::torque; + + // contoller configuration based on the control type + motor.PID_velocity.P = 0.2f; + motor.PID_velocity.I = 20; + motor.PID_velocity.D = 0; + // default voltage_power_supply + motor.voltage_limit = 12; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // angle loop velocity limit + motor.velocity_limit = 50; + + // use monitoring with serial for motor init + // monitoring port + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + + current_sense.linkDriver(&driver); + current_sense.init(); + current_sense.gain_b *= -1; + current_sense.skip_align = true; + motor.linkCurrentSense(¤t_sense); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // set the inital target value + motor.target = 2; + + // define the motor id + command.add('A', onMotor, "motor"); + + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); + + _delay(1000); +} + + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + // velocity, position or voltage + // if tatget not set in parameter uses motor.target variable + motor.move(); + + // user communication + command.run(); +} diff --git a/library.properties b/library.properties index 48b68a6..588cee1 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=SimpleFOCDrivers -version=1.0.1 +version=1.0.2 author=Simplefoc maintainer=Simplefoc sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code. diff --git a/src/encoders/aeat8800q24/AEAT8800Q24.cpp b/src/encoders/aeat8800q24/AEAT8800Q24.cpp index 5c9c235..8be3bd5 100644 --- a/src/encoders/aeat8800q24/AEAT8800Q24.cpp +++ b/src/encoders/aeat8800q24/AEAT8800Q24.cpp @@ -98,5 +98,5 @@ uint8_t AEAT8800Q24::readRegister(uint8_t reg) { }; void AEAT8800Q24::writeRegister(uint8_t reg, uint8_t value) { uint16_t cmd = 0x4000 | ((reg&0x1F)<<8) | value; - uint16_t result = transfer16SPI(cmd); + /*uint16_t result =*/ transfer16SPI(cmd); }; \ No newline at end of file diff --git a/src/encoders/as5047/AS5047.cpp b/src/encoders/as5047/AS5047.cpp index 5c5a67b..905d1b6 100644 --- a/src/encoders/as5047/AS5047.cpp +++ b/src/encoders/as5047/AS5047.cpp @@ -49,7 +49,7 @@ uint16_t AS5047::readCorrectedAngle(){ uint16_t AS5047::readMagnitude(){ uint16_t command = AS5047_MAGNITUDE_REG | AS5047_RW; // set r=1, result is 0x7FFD - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); uint16_t result = nop(); return result; } @@ -62,7 +62,7 @@ bool AS5047::isErrorFlag(){ AS5047Error AS5047::clearErrorFlag(){ uint16_t command = AS5047_ERROR_REG | AS5047_RW; // set r=1, result is 0x4001 - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); uint16_t result = nop(); AS5047Error err = { .framingError = ((result&0x0001)!=0x0000), @@ -75,7 +75,7 @@ AS5047Error AS5047::clearErrorFlag(){ AS5047Settings1 AS5047::readSettings1(){ uint16_t command = AS5047_SETTINGS1_REG | AS5047_PARITY | AS5047_RW; // set r=1, result is 0xC018 - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047Settings1 result = { .reg = nop() }; @@ -85,14 +85,14 @@ AS5047Settings1 AS5047::readSettings1(){ void AS5047::writeSettings1(AS5047Settings1 settings){ uint16_t command = AS5047_SETTINGS1_REG; // set r=0, result is 0x0018 - uint16_t cmdresult = spi_transfer16(command); - cmdresult = spi_transfer16(settings.reg); + /*uint16_t cmdresult =*/ spi_transfer16(command); + /*cmdresult =*/ spi_transfer16(settings.reg); } AS5047Settings2 AS5047::readSettings2(){ uint16_t command = AS5047_SETTINGS2_REG | AS5047_RW; // set r=1, result is 0x4019 - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047Settings2 result = { .reg = nop() }; @@ -102,7 +102,7 @@ AS5047Settings2 AS5047::readSettings2(){ AS5047Diagnostics AS5047::readDiagnostics(){ uint16_t command = AS5047_DIAGNOSTICS_REG | AS5047_PARITY | AS5047_RW; // set r=1, result is 0xFFFC - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047Diagnostics result = { .reg = nop() }; diff --git a/src/encoders/as5047/README.md b/src/encoders/as5047/README.md index c5979a1..22de36b 100644 --- a/src/encoders/as5047/README.md +++ b/src/encoders/as5047/README.md @@ -50,6 +50,9 @@ void setup() { 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(); diff --git a/src/encoders/as5047u/AS5047U.cpp b/src/encoders/as5047u/AS5047U.cpp index ded50aa..60224d1 100644 --- a/src/encoders/as5047u/AS5047U.cpp +++ b/src/encoders/as5047u/AS5047U.cpp @@ -51,7 +51,7 @@ uint16_t AS5047U::readCorrectedAngle(){ uint16_t AS5047U::readMagnitude(){ uint16_t command = AS5047U_MAGNITUDE_REG | AS5047U_RW; - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); uint16_t result = nop(); return result; } @@ -59,7 +59,7 @@ uint16_t AS5047U::readMagnitude(){ uint16_t AS5047U::readVelocity(){ uint16_t command = AS5047U_VELOCITY_REG | AS5047U_RW; - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); uint16_t result = nop(); return result; } @@ -76,7 +76,7 @@ bool AS5047U::isWarningFlag(){ AS5047UError AS5047U::clearErrorFlag(){ uint16_t command = AS5047U_ERROR_REG | AS5047U_RW; // set r=1, result is 0x4001 - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047UError result; result.reg = nop(); return result; @@ -85,7 +85,7 @@ AS5047UError AS5047U::clearErrorFlag(){ AS5047USettings1 AS5047U::readSettings1(){ uint16_t command = AS5047U_SETTINGS1_REG | AS5047U_RW; // set r=1, result is 0xC018 - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047USettings1 result = { .reg = nop() }; @@ -97,8 +97,8 @@ AS5047USettings1 AS5047U::readSettings1(){ void AS5047U::writeSettings1(AS5047USettings1 settings){ uint16_t command = AS5047U_SETTINGS1_REG; // set r=0, result is 0x0018 - uint16_t cmdresult = spi_transfer16(command); - cmdresult = spi_transfer16(settings.reg); + /*uint16_t cmdresult =*/ spi_transfer16(command); + /*cmdresult =*/ spi_transfer16(settings.reg); } @@ -106,7 +106,7 @@ void AS5047U::writeSettings1(AS5047USettings1 settings){ AS5047USettings2 AS5047U::readSettings2(){ uint16_t command = AS5047U_SETTINGS2_REG | AS5047U_RW; // set r=1, result is 0x4019 - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047USettings2 result = { .reg = nop() }; @@ -119,8 +119,8 @@ AS5047USettings2 AS5047U::readSettings2(){ void AS5047U::writeSettings2(AS5047USettings2 settings){ uint16_t command = AS5047U_SETTINGS2_REG; - uint16_t cmdresult = spi_transfer16(command); - cmdresult = spi_transfer16(settings.reg); + /*uint16_t cmdresult =*/ spi_transfer16(command); + /*cmdresult =*/ spi_transfer16(settings.reg); } @@ -129,7 +129,7 @@ void AS5047U::writeSettings2(AS5047USettings2 settings){ AS5047USettings3 AS5047U::readSettings3(){ uint16_t command = AS5047U_SETTINGS3_REG | AS5047U_RW; - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047USettings3 result = { .reg = nop() }; @@ -142,8 +142,8 @@ AS5047USettings3 AS5047U::readSettings3(){ void AS5047U::writeSettings3(AS5047USettings3 settings){ uint16_t command = AS5047U_SETTINGS3_REG; - uint16_t cmdresult = spi_transfer16(command); - cmdresult = spi_transfer16(settings.reg); + /*uint16_t cmdresult =*/ spi_transfer16(command); + /*cmdresult =*/ spi_transfer16(settings.reg); } @@ -151,7 +151,7 @@ void AS5047U::writeSettings3(AS5047USettings3 settings){ AS5047UDiagnostics AS5047U::readDiagnostics(){ uint16_t command = AS5047U_DIAGNOSTICS_REG | AS5047U_RW; - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047UDiagnostics result = { .reg = nop() }; @@ -163,7 +163,7 @@ AS5047UDiagnostics AS5047U::readDiagnostics(){ uint8_t AS5047U::readAGC(){ uint16_t command = AS5047U_AGC_REG | AS5047U_RW; - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); uint16_t result = nop(); return result & 0x00FF; }; @@ -172,7 +172,7 @@ uint8_t AS5047U::readAGC(){ uint8_t AS5047U::readECCCHK(){ uint16_t command = AS5047U_ECCCHK_REG | AS5047U_RW; - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); uint16_t result = nop(); return result & 0x007F; }; @@ -182,7 +182,7 @@ uint8_t AS5047U::readECCCHK(){ AS5047UDisableSettings AS5047U::readDisableSettings(){ uint16_t command = AS5047U_DISABLE_REG | AS5047U_RW; - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047UDisableSettings result = { .reg = nop() }; @@ -193,15 +193,15 @@ AS5047UDisableSettings AS5047U::readDisableSettings(){ void AS5047U::writeDisableSettings(AS5047UDisableSettings settings){ uint16_t command = AS5047U_DISABLE_REG; - uint16_t cmdresult = spi_transfer16(command); - cmdresult = spi_transfer16(settings.reg); + /*uint16_t cmdresult =*/ spi_transfer16(command); + /*cmdresult =*/ spi_transfer16(settings.reg); }; AS5047UECCSettings AS5047U::readECCSettings(){ uint16_t command = AS5047U_ECC_REG | AS5047U_RW; - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5047UECCSettings result = { .reg = nop() }; @@ -212,8 +212,8 @@ AS5047UECCSettings AS5047U::readECCSettings(){ void AS5047U::writeECCSettings(AS5047UECCSettings settings){ uint16_t command = AS5047U_ECC_REG; - uint16_t cmdresult = spi_transfer16(command); - cmdresult = spi_transfer16(settings.reg); + /*uint16_t cmdresult =*/ spi_transfer16(command); + /*cmdresult =*/ spi_transfer16(settings.reg); }; diff --git a/src/encoders/as5047u/README.md b/src/encoders/as5047u/README.md index 211edeb..2a6743c 100644 --- a/src/encoders/as5047u/README.md +++ b/src/encoders/as5047u/README.md @@ -49,6 +49,9 @@ void setup() { 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(); diff --git a/src/encoders/as5048a/AS5048A.cpp b/src/encoders/as5048a/AS5048A.cpp index ae8aaa3..27ef254 100644 --- a/src/encoders/as5048a/AS5048A.cpp +++ b/src/encoders/as5048a/AS5048A.cpp @@ -44,7 +44,7 @@ uint16_t AS5048A::readRawAngle(){ uint16_t AS5048A::readMagnitude(){ uint16_t command = AS5048A_MAGNITUDE_REG | AS5048A_RW; // set r=1, result ix 0x7FFE - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); uint16_t result = nop(); return result; } @@ -57,7 +57,7 @@ bool AS5048A::isErrorFlag(){ AS5048Error AS5048A::clearErrorFlag(){ uint16_t command = AS5048A_ERROR_REG | AS5048A_RW; // set r=1, result ix 0x4001 - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); uint16_t result = nop(); AS5048Error err = { .parityError = ((result&0x0004)!=0x0000), @@ -70,7 +70,7 @@ AS5048Error AS5048A::clearErrorFlag(){ AS5048Diagnostics AS5048A::readDiagnostics(){ uint16_t command = AS5048A_DIAGNOSTICS_REG | AS5048A_RW; // set r=1, result ix 0x7FFD - uint16_t cmdresult = spi_transfer16(command); + /*uint16_t cmdresult =*/ spi_transfer16(command); AS5048Diagnostics result = { .reg = nop() }; diff --git a/src/encoders/as5048a/PreciseMagneticSensorAS5048A.cpp b/src/encoders/as5048a/PreciseMagneticSensorAS5048A.cpp index cd4d818..9f340c4 100644 --- a/src/encoders/as5048a/PreciseMagneticSensorAS5048A.cpp +++ b/src/encoders/as5048a/PreciseMagneticSensorAS5048A.cpp @@ -18,7 +18,7 @@ void PreciseMagneticSensorAS5048A::init(SPIClass* _spi) { this->AS5048A::init(_spi); // velocity calculation init current_ts = _micros(); - uint16_t angle_data = readRawAngle(); + /*uint16_t angle_data =*/ readRawAngle(); current_angle = PreciseAngle(readRawAngle(), 0); getAngle(); } diff --git a/src/encoders/as5048a/README.md b/src/encoders/as5048a/README.md index bf0ea38..468994c 100644 --- a/src/encoders/as5048a/README.md +++ b/src/encoders/as5048a/README.md @@ -49,6 +49,9 @@ void setup() { 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(); diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp new file mode 100644 index 0000000..859e919 --- /dev/null +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -0,0 +1,257 @@ +#include "CalibratedSensor.h" + + +// CalibratedSensor() +// sensor - instance of original sensor object +CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) +{ +}; + + +CalibratedSensor::~CalibratedSensor() +{ + delete calibrationLut; +}; + +// call update of calibrated sensor +void CalibratedSensor::update(){ + _wrapped.update(); + this->Sensor::update(); +}; + +// Retrieve the calibrated sensor angle +void CalibratedSensor::init() { + // assume wrapped sensor has already been initialized + this->Sensor::init(); // call superclass init +} + +// Retrieve the calibrated sensor angle +float CalibratedSensor::getSensorAngle(){ + // raw encoder position e.g. 0-2PI + float rawAngle = _wrapped.getMechanicalAngle(); + + // index of the bucket that rawAngle is part of. + // e.g. rawAngle = 0 --> bucketIndex = 0. + // e.g. rawAngle = 2PI --> bucketIndex = 128. + int bucketIndex = floor(rawAngle/(_2PI/n_lut)); + float remainder = rawAngle - ((_2PI/n_lut)*bucketIndex); + + // Extract the lower and upper LUT value in counts + float y0 = calibrationLut[bucketIndex]; + float y1 = calibrationLut[(bucketIndex+1)%n_lut]; + + // Linear Interpolation Between LUT values y0 and y1 using the remainder + // If remainder = 0, interpolated offset = y0 + // If remainder = 2PI/n_lut, interpolated offset = y1 + float interpolatedOffset = (((_2PI/n_lut)-remainder)/(_2PI/n_lut))*y0 + (remainder/(_2PI/n_lut))*y1; + + // add offset to the raw sensor count. Divide multiply by 2PI/CPR to get radians + float calibratedAngle = rawAngle+interpolatedOffset; + + // return calibrated angle in radians + return calibratedAngle; +} + +void CalibratedSensor::calibrate(BLDCMotor& motor){ + + Serial.println("Starting Sensor Calibration."); + + int _NPP = motor.pole_pairs; // number of pole pairs which is user input + const int n_ticks = 128*_NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) + const int n2_ticks = 40; // increments between saved samples (for smoothing motion) + float deltaElectricalAngle = _2PI*_NPP/(n_ticks*n2_ticks); // Electrical Angle increments for calibration steps + float* error_f = new float[n_ticks](); // pointer to error array rotating forwards + // float* raw_f = new float[n_ticks](); // pointer to raw forward position + float* error_b = new float[n_ticks](); // pointer to error array rotating forwards + // float* raw_b = new float[n_ticks](); // pointer to raw backword position + float* error = new float[n_ticks](); // pointer to error array (average of forward & backward) + float* error_filt = new float[n_ticks](); // pointer to filtered error array (low pass filter) + const int window = 128; // window size for moving average filter of raw error + motor.zero_electric_angle = 0; // Set position sensor offset + + // find natural direction (this is copy of the init code) + // move one electrical revolution forward + for (int i = 0; i <=500; i++ ) { + float angle = _3PI_2 + _2PI * i / 500.0f; + motor.setPhaseVoltage(voltage_calibration, 0, angle); + _wrapped.update(); + _delay(2); + } + // take and angle in the middle + _wrapped.update(); + float mid_angle = _wrapped.getAngle(); + // move one electrical revolution backwards + for (int i = 500; i >=0; i-- ) { + float angle = _3PI_2 + _2PI * i / 500.0f ; + motor.setPhaseVoltage(voltage_calibration, 0, angle); + _wrapped.update(); + _delay(2); + } + _wrapped.update(); + float end_angle = _wrapped.getAngle(); + motor.setPhaseVoltage(0, 0, 0); + _delay(200); + // determine the direction the sensor moved + int directionSensor; + if (mid_angle < end_angle) { + Serial.println("MOT: sensor_direction==CCW"); + directionSensor = -1; + motor.sensor_direction = Direction::CCW; + + } else{ + Serial.println("MOT: sensor_direction==CW"); + directionSensor = 1; + motor.sensor_direction = Direction::CW; + + } + + //Set voltage angle to zero, wait for rotor position to settle + // keep the motor in position while getting the initial positions + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); + _delay(1000); + _wrapped.update(); + float theta_init = _wrapped.getAngle(); + float theta_absolute_init = _wrapped.getMechanicalAngle(); + + /* + Start Calibration + Loop over electrical angles from 0 to NPP*2PI, once forward, once backward + store actual position and error as compared to electrical angle + */ + + /* + forwards rotation + */ + Serial.println("Rotating forwards"); + int k = 0; + for(int i = 0; i n_ticks-1) { + ind -= n_ticks;} + error_filt[i] += error[ind]/(float)window; + } + mean += error_filt[i]/n_ticks; + } + + // calculate offset index + int index_offset = floor(raw_offset/(_2PI/n_lut)); + + // Build Look Up Table + for (int i = 0; i (n_lut-1)){ + ind -= n_lut; + } + if(ind < 0 ){ + ind += n_lut; + } + calibrationLut[ind] = (float) (error_filt[i*_NPP] - mean); + //Serial.print(ind); + //Serial.print('\t'); + //Serial.println(calibrationLut[ind],5); + _delay(1); + } + + // de-allocate memory + delete error_filt; + delete error; + // delete raw_b; + delete error_b; + // delete raw_f; + delete error_f; + + Serial.println("Sensor Calibration Done."); + +} + + diff --git a/src/encoders/calibrated/CalibratedSensor.h b/src/encoders/calibrated/CalibratedSensor.h new file mode 100644 index 0000000..414c53a --- /dev/null +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -0,0 +1,61 @@ +#ifndef __CALIBRATEDSENSOR_H__ +#define __CALIBRATEDSENSOR_H__ + +#include "common/base_classes/Sensor.h" +#include "BLDCMotor.h" +#include "common/base_classes/FOCMotor.h" + + +class CalibratedSensor: public Sensor{ + +public: + // constructor of class with pointer to base class sensor and driver + CalibratedSensor(Sensor& wrapped); + ~CalibratedSensor(); + + /* + Override the update function + */ + virtual void update() override; + + /** + * Calibrate method computes the LUT for the correction + */ + virtual void calibrate(BLDCMotor& motor); + + // voltage to run the calibration: user input + float voltage_calibration = 1; + +protected: + + /** + * getSenorAngle() method of CalibratedSensor class. + * This should call getAngle() on the wrapped instance, and then apply the correction to + * the value returned. + */ + virtual float getSensorAngle() override; + /** + * init method of CaibratedSensor - call after calibration + */ + virtual void init() override; + /** + * delegate instance of Sensor class + */ + Sensor& _wrapped; + + // lut size, currently constan. Perhaps to be made variable by user? + const int n_lut { 128 } ; + // create pointer for lut memory + float* calibrationLut = new float[n_lut](); + + // Init inital angles + float theta_actual { 0.0 }; + float elecAngle { 0.0 }; + float elec_angle { 0.0 }; + float theta_absolute_post { 0.0 }; + float theta_absolute_init { 0.0 }; + float theta_init { 0.0 }; + float avg_elec_angle { 0.0 }; +}; + +#endif \ No newline at end of file diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md new file mode 100644 index 0000000..5768d92 --- /dev/null +++ b/src/encoders/calibrated/README.md @@ -0,0 +1,91 @@ +# Calibrated Sensor + +by [@MarethyuPrefect](https://github.com/MarethyuPrefect) + +A SimpleFOC Sensor wrapper implementation which adds sensor eccentricity calibration. + +Please also see our [forum thread](https://community.simplefoc.com/t/simplefoc-sensor-eccentricity-calibration/2212) on this topic. + + +When you mount your (magnetic) sensor on your frame or motor, there will always be a slight misalignment between magnet and sensor (measurement system). This misalignment between center of rotation and the center of the sensor is called the eccentricity error. + +As a result your measurement system output is non-linear with respect to the rotor of the motor. This will cause an error with respect to the ideal torque you attempt to create with the I_q vector as function of the position. You could interpret this as a disturbance on your control loop which you want to minimize for optimal performance. + +This calibration compensates the sensor reading in a feed forward fashion such that your performance improves. + + +## Hardware setup + +Connect your sensor as usual. Make sure the sensor is working 'normally' i.e. without calibration first. Once things are working and tuned without sensor calibration, you can add the CalibratedSensor to see if you get an improvement. + +Note that during calibration, the motor is turned through several turns, and should be in an unloaded condition. Please ensure your hardware setup can support the motor rotating through full turns. + + +## Softwate setup + +The CalibratedSensor acts as a wrapper to the actual sensor class. When creating the CalibratedSensor object, provide the real +sensor to the constructor of CalibratedSensor. + +First, initialize the real sensor instance as normal. Then, call calibrate() on the CalibratedSensor instance. Then link the +CalibratedSensor to the motor and call motor.initFOC(). + +The motor will then use the calibrated sensor instance. + + +```c++ +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, PB6); +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(11); +BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9); +// instantiate the calibrated sensor, providing the real sensor as a constructor argument +CalibratedSensor sensor_calibrated = CalibratedSensor(sensor); + +void setup() { + sensor.init(); + // Link motor to sensor + motor.linkSensor(&sensor); + // power supply voltage + driver.voltage_power_supply = 20; + driver.init(); + motor.linkDriver(&driver); + // aligning voltage + motor.voltage_sensor_align = 8; + motor.voltage_limit = 20; + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with serial + Serial.begin(115200); + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_variables = _MON_VEL; + motor.monitor_downsample = 10; // default 10 + + // initialize motor + motor.init(); + + // set voltage to run calibration + sensor_calibrated.voltage_calibration = 6; + // Running calibration + sensor_calibrated.calibrate(motor); + + //Serial.println("Calibrating Sensor Done."); + // Linking sensor to motor object + motor.linkSensor(&sensor_calibrated); + + // calibrated init FOC + motor.initFOC(); +} +``` + +Please see the more complete [example](https://github.com/simplefoc/Arduino-FOC-drivers/blob/master/examples/encoders/calibrated/sensor_calibration.ino) in our examples directory. + + +## Roadmap + +Possible future improvements we've thought about: + +- Improve memory usage and performance +- Make calibration able to be saved/restored + diff --git a/src/encoders/ma330/MA330.cpp b/src/encoders/ma330/MA330.cpp new file mode 100644 index 0000000..6e5eac0 --- /dev/null +++ b/src/encoders/ma330/MA330.cpp @@ -0,0 +1,146 @@ +#include "MA330.h" + +MA330::MA330(SPISettings settings, int nCS) : settings(settings), nCS(nCS) { + +}; +MA330::~MA330() { + +}; + +void MA330::init(SPIClass* _spi) { + spi = _spi; + if (nCS >= 0) { + pinMode(nCS, OUTPUT); + digitalWrite(nCS, HIGH); + } +}; + +float MA330::getCurrentAngle() { + return (readRawAngle() * _2PI)/MA330_CPR; +}; // angle in radians, return current value + +uint16_t MA330::readRawAngle() { + uint16_t angle = transfer16(0x0000); + return angle; +}; // 9-14bit angle value + +uint16_t MA330::getZero() { + uint16_t result = readRegister(MA330_REG_ZERO_POSITION_MSB)<<8; + result |= readRegister(MA330_REG_ZERO_POSITION_LSB); + return result; +}; +uint8_t MA330::getBiasCurrentTrimming() { + return readRegister(MA330_REG_BCT); +}; +bool MA330::isBiasCurrrentTrimmingX() { + return (readRegister(MA330_REG_ET) & 0x01)==0x01; +}; +bool MA330::isBiasCurrrentTrimmingY() { + return (readRegister(MA330_REG_ET) & 0x02)==0x02; +}; +uint16_t MA330::getPulsesPerTurn() { + uint16_t result = readRegister(MA330_REG_ILIP_PPT_LSB)>>6; + result |= ((uint16_t)readRegister(MA330_REG_PPT_MSB))<<2; + return result+1; +}; +uint8_t MA330::getIndexLength() { + return (readRegister(MA330_REG_ILIP_PPT_LSB)>>2)&0x0F; +}; +uint8_t MA330::getNumberPolePairs() { + return (readRegister(MA330_REG_NPP)>>5)&0x07;; +}; +uint8_t MA330::getRotationDirection() { + return (readRegister(MA330_REG_RD)>>7); +}; +uint8_t MA330::getFieldStrengthHighThreshold() { + return (readRegister(MA330_REG_MGLT_MGHT)>>2)&0x07; +}; +uint8_t MA330::getFieldStrengthLowThreshold() { + return (readRegister(MA330_REG_MGLT_MGHT)>>5)&0x07; +}; +uint8_t MA330::getFilterWidth() { + return readRegister(MA330_REG_FW); +}; +uint8_t MA330::getHysteresis() { + return readRegister(MA330_REG_HYS); +}; +FieldStrength MA330::getFieldStrength() { + return (FieldStrength)(readRegister(MA330_REG_MGH_MGL)>>6); +}; + + + +void MA330::setZero(uint16_t value) { + writeRegister(MA330_REG_ZERO_POSITION_MSB, value>>8); + writeRegister(MA330_REG_ZERO_POSITION_LSB, value&0x00FF); +}; +void MA330::setBiasCurrentTrimming(uint8_t value) { + writeRegister(MA330_REG_BCT, value); +}; +void MA330::setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled) { + uint8_t val = Xenabled ? 0x01 : 0x00; + val |= (Yenabled ? 0x02 : 0x00); + writeRegister(MA330_REG_ET, val); +}; +void MA330::setPulsesPerTurn(uint16_t value) { + uint16_t pptVal = value - 1; + writeRegister(MA330_REG_PPT_MSB, pptVal>>2); + uint8_t val = readRegister(MA330_REG_ILIP_PPT_LSB); + val &= 0x3F; + val |= (pptVal&0x03)<<6; + writeRegister(MA330_REG_ILIP_PPT_LSB, val); +}; +void MA330::setIndexLength(uint8_t value) { + uint8_t val = readRegister(MA330_REG_ILIP_PPT_LSB); + val &= 0xC0; + val |= ((value<<2)&0x3F); + writeRegister(MA330_REG_ILIP_PPT_LSB, val); +}; +void MA330::setNumberPolePairs(uint8_t value) { + uint8_t val = readRegister(MA330_REG_NPP); + val &= 0x1F; + val |= (value<<5); + writeRegister(MA330_REG_NPP, val); +}; +void MA330::setRotationDirection(uint8_t value) { + if (value==0) + writeRegister(MA330_REG_RD, 0x00); + else + writeRegister(MA330_REG_RD, 0x80); +}; +void MA330::setFilterWidth(uint8_t value) { + writeRegister(MA330_REG_FW, value); +}; +void MA330::setHysteresis(uint8_t value) { + writeRegister(MA330_REG_HYS, value); +}; +void MA330::setFieldStrengthThresholds(uint8_t high, uint8_t low) { + uint8_t val = (low<<5) | (high<<2); + writeRegister(MA330_REG_MGLT_MGHT, val); +}; + + +uint16_t MA330::transfer16(uint16_t outValue) { + if (nCS >= 0) + digitalWrite(nCS, LOW); + spi->beginTransaction(settings); + uint16_t value = spi->transfer16(outValue); + spi->endTransaction(); + if (nCS >= 0) + digitalWrite(nCS, HIGH); + return value; +}; +uint8_t MA330::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 MA330::writeRegister(uint8_t reg, uint8_t value) { + uint16_t cmd = 0x8000 | ((reg&0x1F)<<8) | value; + uint16_t result = transfer16(cmd); + delay(20); // 20ms delay required + result = transfer16(0x0000); + return result>>8; +}; \ No newline at end of file diff --git a/src/encoders/ma330/MA330.h b/src/encoders/ma330/MA330.h new file mode 100644 index 0000000..4b89bc9 --- /dev/null +++ b/src/encoders/ma330/MA330.h @@ -0,0 +1,82 @@ +#ifndef __MA330_H__ +#define __MA330_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 MA330_CPR 65536.0f + +#define MA330_REG_ZERO_POSITION_LSB 0x00 +#define MA330_REG_ZERO_POSITION_MSB 0x01 +#define MA330_REG_BCT 0x02 +#define MA330_REG_ET 0x03 +#define MA330_REG_ILIP_PPT_LSB 0x04 +#define MA330_REG_PPT_MSB 0x05 +#define MA330_REG_MGLT_MGHT 0x06 +#define MA330_REG_NPP 0x07 +#define MA330_REG_RD 0x09 +#define MA330_REG_FW 0x0E +#define MA330_REG_HYS 0x10 +#define MA330_REG_MGH_MGL 0x1B + +#define MA330_BITORDER MSBFIRST + +static SPISettings MA330SPISettings(1000000, MA330_BITORDER, SPI_MODE3); // @suppress("Invalid arguments") + +class MA330 { +public: + MA330(SPISettings settings = MA330SPISettings, int nCS = -1); + virtual ~MA330(); + + virtual void init(SPIClass* _spi = &SPI); + + float getCurrentAngle(); // angle in radians, return current value + + uint16_t readRawAngle(); // 9-14bit angle value + + uint16_t getZero(); + uint8_t getBiasCurrentTrimming(); + bool isBiasCurrrentTrimmingX(); + bool isBiasCurrrentTrimmingY(); + uint16_t getPulsesPerTurn(); + uint8_t getIndexLength(); + uint8_t getNumberPolePairs(); + uint8_t getRotationDirection(); + uint8_t getFilterWidth(); + uint8_t getHysteresis(); + uint8_t getFieldStrengthHighThreshold(); + uint8_t getFieldStrengthLowThreshold(); + FieldStrength getFieldStrength(); + + void setZero(uint16_t); + void setBiasCurrentTrimming(uint8_t); + void setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled); + void setPulsesPerTurn(uint16_t); + void setIndexLength(uint8_t); + void setNumberPolePairs(uint8_t); + void setRotationDirection(uint8_t); + void setFilterWidth(uint8_t); + void setHysteresis(uint8_t); + void setFieldStrengthThresholds(uint8_t high, uint8_t low); + +private: + SPIClass* spi; + SPISettings settings; + int nCS = -1; + + uint16_t transfer16(uint16_t outValue); + uint8_t readRegister(uint8_t reg); + uint8_t writeRegister(uint8_t reg, uint8_t value); +}; + +#endif \ No newline at end of file diff --git a/src/encoders/ma330/MagneticSensorMA330.cpp b/src/encoders/ma330/MagneticSensorMA330.cpp new file mode 100644 index 0000000..a884065 --- /dev/null +++ b/src/encoders/ma330/MagneticSensorMA330.cpp @@ -0,0 +1,26 @@ +#include "./MagneticSensorMA330.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + +MagneticSensorMA330::MagneticSensorMA330(int nCS, SPISettings settings) : MA330(settings, nCS) { + +} + + +MagneticSensorMA330::~MagneticSensorMA330(){ + +} + + +void MagneticSensorMA330::init(SPIClass* _spi) { + this->MA330::init(_spi); + this->Sensor::init(); +} + + +float MagneticSensorMA330::getSensorAngle() { + float angle_data = readRawAngle(); + angle_data = ( angle_data / (float)MA330_CPR) * _2PI; + // return the shaft angle + return angle_data; +} diff --git a/src/encoders/ma330/MagneticSensorMA330.h b/src/encoders/ma330/MagneticSensorMA330.h new file mode 100644 index 0000000..1befbdf --- /dev/null +++ b/src/encoders/ma330/MagneticSensorMA330.h @@ -0,0 +1,19 @@ +#ifndef __MAGNETIC_SENSOR_MA330_H__ +#define __MAGNETIC_SENSOR_MA330_H__ + + +#include "common/base_classes/Sensor.h" +#include "./MA330.h" + +class MagneticSensorMA330 : public Sensor, public MA330 { +public: + MagneticSensorMA330(int nCS = -1, SPISettings settings = MA330SPISettings); + virtual ~MagneticSensorMA330(); + + virtual float getSensorAngle() override; + + virtual void init(SPIClass* _spi = &SPI); +}; + + +#endif \ No newline at end of file diff --git a/src/encoders/ma330/README.md b/src/encoders/ma330/README.md new file mode 100644 index 0000000..fcf635d --- /dev/null +++ b/src/encoders/ma330/README.md @@ -0,0 +1,66 @@ +# MA330 SimpleFOC driver + +While MA330 absolute position magnetic rotary encoder is supported by the standard MagneticSensorSPI driver included in the base distribution, this MA330-specific driver includes some optimisations: + +- access to the other registers of the MA330 +- this driver directly reads the angle with one call to SPI +- this will halve the number of 16-bit SPI transfers per simpleFOC loop iteration + + +## 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/MA330/MagneticSensorMA330.h" + +#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin +MagneticSensorMA330 sensor1(SENSOR1_CS); + + +void setup() { + sensor1.init(); +} +``` + +Set some options: + +```c++ +MagneticSensorMA330 sensor1(SENSOR1_CS, true, 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(); +``` diff --git a/src/encoders/ma730/MA730.cpp b/src/encoders/ma730/MA730.cpp index 885e335..7a1b7b8 100644 --- a/src/encoders/ma730/MA730.cpp +++ b/src/encoders/ma730/MA730.cpp @@ -94,7 +94,7 @@ void MA730::setRotationDirection(uint8_t value) { writeRegister(MA730_REG_RD, 0x80); }; void MA730::setFieldStrengthThresholds(uint8_t high, uint8_t low) { - uint8_t val = (low<<5) | (high&0x07); + uint8_t val = (low<<5) | (high<<2); writeRegister(MA730_REG_MGLT_MGHT, val); }; diff --git a/src/encoders/ma730/MA730.h b/src/encoders/ma730/MA730.h index 00c6e48..56b0b5a 100644 --- a/src/encoders/ma730/MA730.h +++ b/src/encoders/ma730/MA730.h @@ -5,7 +5,7 @@ #include "Arduino.h" #include "SPI.h" -enum FieldStrength { +enum FieldStrength : uint8_t { FS_NORMAL = 0x00, FS_LOW = 0x01, FS_HIGH = 0x02, diff --git a/src/encoders/ma730/README.md b/src/encoders/ma730/README.md index d2aabed..4602eee 100644 --- a/src/encoders/ma730/README.md +++ b/src/encoders/ma730/README.md @@ -49,6 +49,9 @@ void setup() { 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(); diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp new file mode 100644 index 0000000..01e8d97 --- /dev/null +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp @@ -0,0 +1,42 @@ +#include "./MagneticSensorMT6701SSI.h" +#include "common/foc_utils.h" +#include "common/time_utils.h" + +MagneticSensorMT6701SSI::MagneticSensorMT6701SSI(int nCS, SPISettings settings) : settings(settings), nCS(nCS) { + +} + + +MagneticSensorMT6701SSI::~MagneticSensorMT6701SSI() { + +} + +void MagneticSensorMT6701SSI::init(SPIClass* _spi) { + this->spi=_spi; + if (nCS >= 0) { + pinMode(nCS, OUTPUT); + digitalWrite(nCS, HIGH); + } + this->spi->begin(); + this->Sensor::init(); +} + +// check 40us delay between each read? +float MagneticSensorMT6701SSI::getSensorAngle() { + float angle_data = readRawAngleSSI(); + angle_data = ( angle_data / (float)MT6701_CPR ) * _2PI; + // return the shaft angle + return angle_data; +} + + +uint16_t MagneticSensorMT6701SSI::readRawAngleSSI() { + if (nCS >= 0) + digitalWrite(nCS, LOW); + spi->beginTransaction(settings); + uint16_t value = spi->transfer16(0x0000); + spi->endTransaction(); + if (nCS >= 0) + digitalWrite(nCS, HIGH); + return (value>>MT6701_DATA_POS)&0x3FFF; +}; diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.h b/src/encoders/mt6701/MagneticSensorMT6701SSI.h new file mode 100644 index 0000000..755d733 --- /dev/null +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.h @@ -0,0 +1,43 @@ +#ifndef __MAGNETIC_SENSOR_MT6701_SSI_H__ +#define __MAGNETIC_SENSOR_MT6701_SSI_H__ + +#include "Arduino.h" +#include "SPI.h" +#include "common/base_classes/Sensor.h" + + +#define MT6701_CPR 16384.0f + +#define MT6701_BITORDER MSBFIRST + +#if defined(TARGET_RP2040) +#define MT6701_DATA_POS 1 +#else +#define MT6701_DATA_POS 2 +#endif + +// Use SPI mode 2, capture on falling edge. First bit is not valid data, so have to read 25 bits to get a full SSI frame. +// SSI frame is 1 bit ignore, 14 bits angle, 4 bit status and 6 bit CRC. +static SPISettings MT6701SSISettings(1000000, MT6701_BITORDER, SPI_MODE2); // @suppress("Invalid arguments") + + + +class MagneticSensorMT6701SSI : public Sensor { +public: + MagneticSensorMT6701SSI(int nCS = -1, SPISettings settings = MT6701SSISettings); + virtual ~MagneticSensorMT6701SSI(); + + virtual void init(SPIClass* _spi = &SPI); + + float getSensorAngle() override; // angle in radians, return current value + +protected: + uint16_t readRawAngleSSI(); + + SPISettings settings; + SPIClass* spi; + int nCS = -1; +}; + + +#endif \ No newline at end of file diff --git a/src/encoders/mt6701/README.md b/src/encoders/mt6701/README.md new file mode 100644 index 0000000..df04808 --- /dev/null +++ b/src/encoders/mt6701/README.md @@ -0,0 +1,82 @@ +# MT6701 SimpleFOC driver + +:warning: work in progress... SSI driver is working. I2C not yet complete. + +Due to the peculiarities of its interfaces, this very versatile sensor is not directly supported by SimpleFOC's SPI or I2C magnetic sensor implementations. This folder contains dedicated SimpleFOC sensor drivers for the MT6701 for I2C and SSI. + +Note: the ABZ, UVW and Analog outputs of this sensor are supported by the standard SimpleFOC encoder, hall-sensor or analog sensor classes respectively. + +:warning: Note: the I2C output of this sensor is probably too slow for high performance motor control, but could be useful to program the sensor IC, and to read the absolute angle values for intializing ABZ or UVW modes. + +## Hardware setup + +For I2C, connect the sensor as normal for I2C, using the SDA and SCL lines. + +:warning: Note: to program the sensor via I2C, it has to be operated at 5V. + +For SSI, connect the CSN line to the nCS output of your MCU, the CLK line to the SCLK output of the MCU, and the DO line to the CIPO (MISO) input of the MCU. + +In my tests, the sensor was not able to work correctly together with other SPI devices on the same bus, but your experience might differ from mine. + +## Software setup + +### SSI code sample + +```c++ +#include "Arduino.h" +#include "SPI.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "encoders/MT6701/MagneticSensorMT6701SSI.h" + + +#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin +MagneticSensorMT6701SSI sensor1(SENSOR1_CS); + + +void setup() { + sensor1.init(); +} +``` + +To use a custom SPI bus: + +```c++ +void setup() { + sensor1.init(&SPI2); +} +``` + + + +### I2C code sample + +I2C usage is quite simple: + +```c++ +#include "Arduino.h" +#include "Wire.h" +#include "SimpleFOC.h" +#include "SimpleFOCDrivers.h" +#include "encoders/MT6701/MagneticSensorMT6701I2C.h" + +MagneticSensorMT6701I2C sensor1(); + + +void setup() { + sensor1.init(); +} + +``` + +If you've programmed a different I2C address or want to use a different I2C bus you can: + +```c++ +#define I2C_ADDR 0x70 +MagneticSensorMT6701I2C sensor1(I2C_ADDR); + + +void setup() { + sensor1.init(&Wire2); +} +``` diff --git a/src/encoders/mt6816/MT6816.cpp b/src/encoders/mt6816/MT6816.cpp new file mode 100644 index 0000000..d874bad --- /dev/null +++ b/src/encoders/mt6816/MT6816.cpp @@ -0,0 +1,56 @@ + +#include "MT6816.h" + +MT6816::MT6816(SPISettings settings, int nCS) : settings(settings), nCS(nCS) { +}; + +MT6816::~MT6816() { +}; + +void MT6816::init(SPIClass* _spi) { + spi = _spi; + if (nCS >= 0) { + pinMode(nCS, OUTPUT); + digitalWrite(nCS, HIGH); + spi->begin(); + } +}; + +uint16_t MT6816::readRawAngle() { + uint16_t angle_data = 0; + angle_data = spi_transfer16(MT6816_READ_REG_03) << 8; + angle_data |= spi_transfer16(MT6816_READ_REG_04); + + if ((angle_data & MT6816_NO_MAGNET_WARNING_BIT) == MT6816_NO_MAGNET_WARNING_BIT) { + this->no_magnetic_reading = true; + } else { + this->no_magnetic_reading = false; + } + + if (!this->parityCheck(angle_data)) { + return 0; + } + + return (angle_data >> 2); +} + +bool MT6816::parityCheck(uint16_t data) { + data ^= data >> 8; + data ^= data >> 4; + data ^= data >> 2; + data ^= data >> 1; + + return (~data) & 1; +} + +uint16_t MT6816::spi_transfer16(uint16_t outdata) { + if (nCS>=0) + digitalWrite(nCS, 0); + spi->beginTransaction(settings); + uint16_t result = spi->transfer16(outdata); + spi->endTransaction(); + if (nCS>=0) + digitalWrite(nCS, 1); + + return result; +} diff --git a/src/encoders/mt6816/MT6816.h b/src/encoders/mt6816/MT6816.h new file mode 100644 index 0000000..68b448a --- /dev/null +++ b/src/encoders/mt6816/MT6816.h @@ -0,0 +1,40 @@ + +#ifndef MT6816_H +#define MT6816_H + +#include "Arduino.h" +#include "SPI.h" + +#define _2PI 6.28318530718f +#define MT6816_CPR 16384.0f + +#define MT6816_READ_REG_03 0x8300 +#define MT6816_READ_REG_04 0x8400 + +#define MT6816_NO_MAGNET_WARNING_BIT 0x0002 +#define MT6816_BITORDER MSBFIRST + + +static SPISettings MT6816SPISettings(1000000, MT6816_BITORDER, SPI_MODE3); + +class MT6816 { +public: + MT6816(SPISettings settings = MT6816SPISettings, int nCS = -1); + virtual ~MT6816(); + + virtual void init(SPIClass* _spi = &SPI); + uint16_t readRawAngle(); + bool isNoMagneticReading() { + return no_magnetic_reading; + } + +private: + bool parityCheck(uint16_t data); + uint16_t spi_transfer16(uint16_t outdata); + SPIClass* spi; + SPISettings settings; + bool no_magnetic_reading = false; + int nCS = -1; +}; + +#endif /* MT6816_H */ diff --git a/src/encoders/mt6816/MagneticSensorMT6816.cpp b/src/encoders/mt6816/MagneticSensorMT6816.cpp new file mode 100644 index 0000000..4167d78 --- /dev/null +++ b/src/encoders/mt6816/MagneticSensorMT6816.cpp @@ -0,0 +1,26 @@ + +#include "common/foc_utils.h" +#include "common/time_utils.h" +#include "MagneticSensorMT6816.h" + + +MagneticSensorMT6816::MagneticSensorMT6816(int nCS, SPISettings settings) : MT6816(settings, nCS) { +} + +MagneticSensorMT6816::~MagneticSensorMT6816(){ +} + +void MagneticSensorMT6816::init(SPIClass* _spi) { + this->MT6816::init(_spi); + this->Sensor::init(); +} + +float MagneticSensorMT6816::getSensorAngle() { + uint16_t raw_angle_data = readRawAngle(); + + if (this->MT6816::isNoMagneticReading()) { + return 0; + } + + return static_cast(raw_angle_data) / MT6816_CPR * _2PI; +} \ No newline at end of file diff --git a/src/encoders/mt6816/MagneticSensorMT6816.h b/src/encoders/mt6816/MagneticSensorMT6816.h new file mode 100644 index 0000000..51628fc --- /dev/null +++ b/src/encoders/mt6816/MagneticSensorMT6816.h @@ -0,0 +1,17 @@ + +#ifndef MAGNETICSENSOR_MT6816_H +#define MAGNETICSENSOR_MT6816_H + +#include "common/base_classes/Sensor.h" +#include "MT6816.h" + +class MagneticSensorMT6816 : public Sensor, public MT6816 { +public: + MagneticSensorMT6816(int nCS = -1, SPISettings settings = MT6816SPISettings); + virtual ~MagneticSensorMT6816(); + + virtual float getSensorAngle() override; + virtual void init(SPIClass* _spi = &SPI); +}; + +#endif /* MAGNETICSENSOR_MT6816_H */