diff --git a/README.md b/README.md index 19f65bc..f722312 100644 --- a/README.md +++ b/README.md @@ -28,6 +28,7 @@ What's here? See the sections below. Each driver or function should come with it - [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. ### Communications 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/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 */