From 2f16887138832ab982f8735fa25885c4acd60212 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 31 May 2022 14:38:07 +0200 Subject: [PATCH 01/31] add directory for calibrated sensor code --- src/encoders/calibrated/README.md | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 src/encoders/calibrated/README.md diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md new file mode 100644 index 0000000..a3ea59f --- /dev/null +++ b/src/encoders/calibrated/README.md @@ -0,0 +1,4 @@ +# Calibrated Sensor + +by [@MarethyuPrefect](https://github.com/MarethyuPrefect) + From f0c1576e512fba188dd66d58cc2d679540306257 Mon Sep 17 00:00:00 2001 From: MarethyuPrefect Date: Tue, 31 May 2022 21:02:22 +0200 Subject: [PATCH 02/31] added CalibratedSensor class --- src/encoders/calibrated/CalibratedSensor.cpp | 248 +++++++++++++++++++ src/encoders/calibrated/CalibratedSensor.h | 60 +++++ 2 files changed, 308 insertions(+) create mode 100644 src/encoders/calibrated/CalibratedSensor.cpp create mode 100644 src/encoders/calibrated/CalibratedSensor.h diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp new file mode 100644 index 0000000..e4fbae0 --- /dev/null +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -0,0 +1,248 @@ +#include "CalibratedSensor.h" + + +// CalibratedSensor() +// sensor - instance of original sensor object +CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) +{ +}; + +// 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 + for(int i = 0; i<40000; i++) + { + motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); + } + + // 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); + } + + 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..533043a --- /dev/null +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -0,0 +1,60 @@ +#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); + + /* + 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 From bdd573d19b810cf93469bb9117ebc636abd334f7 Mon Sep 17 00:00:00 2001 From: MarethyuPrefect Date: Wed, 1 Jun 2022 08:57:28 +0200 Subject: [PATCH 03/31] Added example for sensor eccentricity calibration --- .../calibrated/sensor_calibration.ino | 84 +++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100644 examples/encoders/calibrated/sensor_calibration.ino diff --git a/examples/encoders/calibrated/sensor_calibration.ino b/examples/encoders/calibrated/sensor_calibration.ino new file mode 100644 index 0000000..1a766cf --- /dev/null +++ b/examples/encoders/calibrated/sensor_calibration.ino @@ -0,0 +1,84 @@ +/** + * 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 + +// 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 From 6dd6745cbe3c40b3724889361121c6a9203370e7 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 18 Jun 2022 17:30:31 +0200 Subject: [PATCH 04/31] README updates --- src/encoders/as5047/README.md | 3 +++ src/encoders/as5047u/README.md | 3 +++ src/encoders/as5048a/README.md | 3 +++ src/encoders/ma730/README.md | 3 +++ 4 files changed, 12 insertions(+) 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/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/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/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(); From 5cbaaf7f0500161beab25aea0db8aaf70ce85337 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 18 Jun 2022 17:30:55 +0200 Subject: [PATCH 05/31] small fix in MA730 driver --- src/encoders/ma730/MA730.cpp | 2 +- src/encoders/ma730/MA730.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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, From ad4ea6c3d5c02ff59f5b2d76f2f32f27567db532 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 18 Jun 2022 17:31:28 +0200 Subject: [PATCH 06/31] MA330 driver --- src/encoders/ma330/MA330.cpp | 146 +++++++++++++++++++++ src/encoders/ma330/MA330.h | 82 ++++++++++++ src/encoders/ma330/MagneticSensorMA330.cpp | 26 ++++ src/encoders/ma330/MagneticSensorMA330.h | 19 +++ src/encoders/ma330/README.md | 66 ++++++++++ 5 files changed, 339 insertions(+) create mode 100644 src/encoders/ma330/MA330.cpp create mode 100644 src/encoders/ma330/MA330.h create mode 100644 src/encoders/ma330/MagneticSensorMA330.cpp create mode 100644 src/encoders/ma330/MagneticSensorMA330.h create mode 100644 src/encoders/ma330/README.md 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(); +``` From 2fb7ecbc8605902a3a6fcf62eb5407d9ed0ae2d6 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 18 Jun 2022 17:31:49 +0200 Subject: [PATCH 07/31] add MA330 driver to main README --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 46f7983..19f65bc 100644 --- a/README.md +++ b/README.md @@ -27,6 +27,7 @@ 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. ### Communications From 61c32504728543d5674aa2357f5032ff36505cc9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 19 Jun 2022 20:31:43 +0200 Subject: [PATCH 08/31] commented unused array, removed redundant section --- src/encoders/calibrated/CalibratedSensor.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index e4fbae0..f4b27e3 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -55,9 +55,9 @@ void CalibratedSensor::calibrate(BLDCMotor& motor){ 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* 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* 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 @@ -100,11 +100,6 @@ void CalibratedSensor::calibrate(BLDCMotor& motor){ } //Set voltage angle to zero, wait for rotor position to settle - for(int i = 0; i<40000; i++) - { - motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); - } - // keep the motor in position while getting the initial positions motor.setPhaseVoltage(voltage_calibration, 0, elec_angle); _delay(1000); @@ -146,7 +141,7 @@ void CalibratedSensor::calibrate(BLDCMotor& motor){ error_f[i] = elec_angle/_NPP - theta_actual; } // if overflow happened track it as full rotation - raw_f[i] = theta_actual; +// raw_f[i] = theta_actual; // storing the normalized angle every time the electrical angle 3PI/2 to calculate average zero electrical angle if(i==(k*128+96)) @@ -184,7 +179,7 @@ void CalibratedSensor::calibrate(BLDCMotor& motor){ { error_b[i] = elec_angle/_NPP - theta_actual; } - raw_b[i] = theta_actual; +// raw_b[i] = theta_actual; } // get post calibration mechanical angle. From 8686b8cc0630ebb78b659b9c9632e8f1065f0453 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sun, 19 Jun 2022 21:12:54 +0200 Subject: [PATCH 09/31] de-allocate memory when finished with it --- src/encoders/calibrated/CalibratedSensor.cpp | 14 ++++++++++++++ src/encoders/calibrated/CalibratedSensor.h | 1 + 2 files changed, 15 insertions(+) diff --git a/src/encoders/calibrated/CalibratedSensor.cpp b/src/encoders/calibrated/CalibratedSensor.cpp index f4b27e3..859e919 100644 --- a/src/encoders/calibrated/CalibratedSensor.cpp +++ b/src/encoders/calibrated/CalibratedSensor.cpp @@ -7,6 +7,12 @@ CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) { }; + +CalibratedSensor::~CalibratedSensor() +{ + delete calibrationLut; +}; + // call update of calibrated sensor void CalibratedSensor::update(){ _wrapped.update(); @@ -236,6 +242,14 @@ void CalibratedSensor::calibrate(BLDCMotor& motor){ _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 index 533043a..414c53a 100644 --- a/src/encoders/calibrated/CalibratedSensor.h +++ b/src/encoders/calibrated/CalibratedSensor.h @@ -11,6 +11,7 @@ class CalibratedSensor: public Sensor{ public: // constructor of class with pointer to base class sensor and driver CalibratedSensor(Sensor& wrapped); + ~CalibratedSensor(); /* Override the update function From 2c3d0c1f3c33f9759b92cf19589274e9c5dadf0e Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 26 Jul 2022 23:56:06 +0200 Subject: [PATCH 10/31] added README for CalibratedSensor --- src/encoders/calibrated/README.md | 87 +++++++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) diff --git a/src/encoders/calibrated/README.md b/src/encoders/calibrated/README.md index a3ea59f..5768d92 100644 --- a/src/encoders/calibrated/README.md +++ b/src/encoders/calibrated/README.md @@ -2,3 +2,90 @@ 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 + From bd9dfef9fb3e257dee681dba0a8c18100364e5cf Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Tue, 26 Jul 2022 23:56:51 +0200 Subject: [PATCH 11/31] fix linting and example test compilation errors --- .github/workflows/ccpp.yml | 7 ++ examples/encoders/calibrated/calibrated.ino | 86 +++++++++++++++++++++ 2 files changed, 93 insertions(+) create mode 100644 examples/encoders/calibrated/calibrated.ino diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 4370526..19a6b5e 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -27,20 +27,25 @@ jobs: include: - arduino-boards-fqbn: arduino:avr:uno sketch-names: '**.ino' + sketches-exclude: calibrated 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' + 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 @@ -49,10 +54,12 @@ jobs: 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' + sketches-exclude: calibrated - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json required-libraries: Simple FOC 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 From df81a4da9bc54b0afea42452a961d9e2953b9bee Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 27 Jul 2022 00:00:07 +0200 Subject: [PATCH 12/31] changed filename of example for linting --- .../calibrated/sensor_calibration.ino | 84 ------------------- 1 file changed, 84 deletions(-) delete mode 100644 examples/encoders/calibrated/sensor_calibration.ino diff --git a/examples/encoders/calibrated/sensor_calibration.ino b/examples/encoders/calibrated/sensor_calibration.ino deleted file mode 100644 index 1a766cf..0000000 --- a/examples/encoders/calibrated/sensor_calibration.ino +++ /dev/null @@ -1,84 +0,0 @@ -/** - * 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 - -// 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 From b139cf913c231c9f9b69c8a9aa1b0419d115d6bc Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 27 Jul 2022 00:07:30 +0200 Subject: [PATCH 13/31] fix github compile actions --- .github/workflows/ccpp.yml | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 19a6b5e..cfcd26a 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -26,25 +26,20 @@ jobs: - arduino:mbed_rp2040:pico # rpi pico include: - arduino-boards-fqbn: arduino:avr:uno - sketch-names: '**.ino' sketches-exclude: calibrated 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 @@ -53,21 +48,17 @@ 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' sketches-exclude: calibrated - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_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 required-libraries: Simple FOC - sketch-names: '**.ino' # Do not cancel all jobs / architectures if one job fails fail-fast: false steps: From a2c3bc6846cf6e57a8a6919729bdb1accc35655b Mon Sep 17 00:00:00 2001 From: Wang Gang Date: Tue, 9 Aug 2022 10:20:32 +0800 Subject: [PATCH 14/31] Add MT6816 magnetic sensor SPI driver along with an example code Format code indentation --- README.md | 1 + .../encoders/mt6816/mt6816_spi/mt6816_spi.ino | 106 ++++++++++++++++++ src/encoders/mt6816/MT6816.cpp | 56 +++++++++ src/encoders/mt6816/MT6816.h | 40 +++++++ src/encoders/mt6816/MagneticSensorMT6816.cpp | 26 +++++ src/encoders/mt6816/MagneticSensorMT6816.h | 17 +++ 6 files changed, 246 insertions(+) create mode 100644 examples/encoders/mt6816/mt6816_spi/mt6816_spi.ino create mode 100644 src/encoders/mt6816/MT6816.cpp create mode 100644 src/encoders/mt6816/MT6816.h create mode 100644 src/encoders/mt6816/MagneticSensorMT6816.cpp create mode 100644 src/encoders/mt6816/MagneticSensorMT6816.h 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 */ From 427933e271729331e712293f73d07bd589408377 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 13 Aug 2022 21:09:07 +0200 Subject: [PATCH 15/31] MT6701 SSI support --- .../mt6701/MagneticSensorMT6701SSI.cpp | 41 ++++++++++ src/encoders/mt6701/MagneticSensorMT6701SSI.h | 37 +++++++++ src/encoders/mt6701/README.md | 82 +++++++++++++++++++ 3 files changed, 160 insertions(+) create mode 100644 src/encoders/mt6701/MagneticSensorMT6701SSI.cpp create mode 100644 src/encoders/mt6701/MagneticSensorMT6701SSI.h create mode 100644 src/encoders/mt6701/README.md diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp new file mode 100644 index 0000000..fd86d69 --- /dev/null +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp @@ -0,0 +1,41 @@ +#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; + this->Sensor::init(); + if (nCS >= 0) { + pinMode(nCS, OUTPUT); + digitalWrite(nCS, HIGH); + } +} + +// 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>>2)&0x3FFF; +}; diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.h b/src/encoders/mt6701/MagneticSensorMT6701SSI.h new file mode 100644 index 0000000..29e6aed --- /dev/null +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.h @@ -0,0 +1,37 @@ +#ifndef __MAGNETIC_SENSOR_MT6701_SSI_H__ +#define __MAGNETIC_SENSOR_MT6701_SSI_H__ + +#include "SPI.h" +#include "common/base_classes/Sensor.h" + + +#define MT6701_CPR 16384.0f + +#define MT6701_BITORDER MSBFIRST + + +// 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); +} +``` From 43c122d2e5794d101ac8e8b9d62797d4e772ea51 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 13 Aug 2022 21:12:42 +0200 Subject: [PATCH 16/31] add MT6701 to main README --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index f722312..d481b47 100644 --- a/README.md +++ b/README.md @@ -29,6 +29,7 @@ What's here? See the sections below. Each driver or function should come with it - [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 From 5fa46f36e6c64e2acc7ca1a2d85385248d1bcde4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 19 Aug 2022 00:33:12 +0200 Subject: [PATCH 17/31] fix initialization of MT6701 SSI mode --- src/encoders/mt6701/MagneticSensorMT6701SSI.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp index fd86d69..ba26e2e 100644 --- a/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp @@ -13,11 +13,12 @@ MagneticSensorMT6701SSI::~MagneticSensorMT6701SSI() { void MagneticSensorMT6701SSI::init(SPIClass* _spi) { this->spi=_spi; - this->Sensor::init(); if (nCS >= 0) { pinMode(nCS, OUTPUT); digitalWrite(nCS, HIGH); } + this->spi->begin(); + this->Sensor::init(); } // check 40us delay between each read? From a5866700c375bd9d04c23ccb09b468499cd614c4 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 19 Aug 2022 00:44:19 +0200 Subject: [PATCH 18/31] fix MT6701 sensor on RP2040 SPI on RP2040 seems to read an extra bit from MT6701, which STM32 does not seem to read. --- src/encoders/mt6701/MagneticSensorMT6701SSI.cpp | 2 +- src/encoders/mt6701/MagneticSensorMT6701SSI.h | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp index ba26e2e..01e8d97 100644 --- a/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp @@ -38,5 +38,5 @@ uint16_t MagneticSensorMT6701SSI::readRawAngleSSI() { spi->endTransaction(); if (nCS >= 0) digitalWrite(nCS, HIGH); - return (value>>2)&0x3FFF; + return (value>>MT6701_DATA_POS)&0x3FFF; }; diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.h b/src/encoders/mt6701/MagneticSensorMT6701SSI.h index 29e6aed..d8e165a 100644 --- a/src/encoders/mt6701/MagneticSensorMT6701SSI.h +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.h @@ -9,6 +9,11 @@ #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. From 966ac9ca25d05c2ee8c25560a823e8009e4ca9b3 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Fri, 26 Aug 2022 16:26:18 +0200 Subject: [PATCH 19/31] add header missing for compile on ESP32 --- src/encoders/mt6701/MagneticSensorMT6701SSI.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/encoders/mt6701/MagneticSensorMT6701SSI.h b/src/encoders/mt6701/MagneticSensorMT6701SSI.h index d8e165a..755d733 100644 --- a/src/encoders/mt6701/MagneticSensorMT6701SSI.h +++ b/src/encoders/mt6701/MagneticSensorMT6701SSI.h @@ -1,6 +1,7 @@ #ifndef __MAGNETIC_SENSOR_MT6701_SSI_H__ #define __MAGNETIC_SENSOR_MT6701_SSI_H__ +#include "Arduino.h" #include "SPI.h" #include "common/base_classes/Sensor.h" From f996220444c4d33e59e1ab3071b56cdfa66b0ee5 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 16:40:09 +0200 Subject: [PATCH 20/31] Update library version to 1.0.2 --- .gitignore | 1 + README.md | 9 +++++++++ library.properties | 2 +- 3 files changed, 11 insertions(+), 1 deletion(-) 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 d481b47..2422249 100644 --- a/README.md +++ b/README.md @@ -8,6 +8,15 @@ 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. +## New Release + +v1.0.2 - Released Oct 2022, for Simple FOC 2.2.3 + +- Calibrated sensor by @MarethyuPrefect +- New Sensors: MT6701, MA330, MT6816 +- Fixes bugs + + ## What's contained What's here? See the sections below. Each driver or function should come with its own more detailed README. 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. From 7793caf6d4bed2bc9d3b80f3ddb7b208594b6871 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:17:46 +0200 Subject: [PATCH 21/31] remove mt6816 sketch from uno test cases - too big --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index cfcd26a..8d96cd6 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -26,8 +26,8 @@ jobs: - arduino:mbed_rp2040:pico # rpi pico include: - arduino-boards-fqbn: arduino:avr:uno - sketches-exclude: calibrated - required-libraries: Simple FOC + sketches-exclude: calibrated mt6816_spi + required-libraries: Simple FOCs - arduino-boards-fqbn: arduino:sam:arduino_due_x required-libraries: Simple FOC sketches-exclude: calibrated From 4f683f277a9acc8766edfae68f65a14ea0a49c98 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:18:26 +0200 Subject: [PATCH 22/31] update stm32 board manager url --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 8d96cd6..7f3dc78 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -54,10 +54,10 @@ jobs: required-libraries: Simple FOC sketches-exclude: calibrated - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE - platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/master/STM32/package_stm_index.json + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json required-libraries: Simple FOC # Do not cancel all jobs / architectures if one job fails fail-fast: false From 9b7daa9a1993d3313dd154b99d49a33d060f0269 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:20:20 +0200 Subject: [PATCH 23/31] fix typo in library name --- .github/workflows/ccpp.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 7f3dc78..6769dcf 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -27,7 +27,7 @@ jobs: include: - arduino-boards-fqbn: arduino:avr:uno sketches-exclude: calibrated mt6816_spi - required-libraries: Simple FOCs + required-libraries: Simple FOC - arduino-boards-fqbn: arduino:sam:arduino_due_x required-libraries: Simple FOC sketches-exclude: calibrated From dff83b0ceab39913de8ece1d51335581e49a6d86 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:28:12 +0200 Subject: [PATCH 24/31] fix stm32 npard fqdns --- .github/workflows/ccpp.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 6769dcf..a3b75cb 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -53,10 +53,10 @@ jobs: platform-url: https://raw.githubusercontent.com/espressif/arduino-esp32/gh-pages/package_esp32_dev_index.json required-libraries: Simple FOC sketches-exclude: calibrated - - arduino-boards-fqbn: STM32:stm32:GenF1:pnum=BLUEPILL_F103C8 + - 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 - - arduino-boards-fqbn: STM32:stm32:Nucleo_64:pnum=NUCLEO_F411RE + - 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 # Do not cancel all jobs / architectures if one job fails From d66ce7e4f64ee9d9de1ade8ffa0d7d97573c1a51 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:39:13 +0200 Subject: [PATCH 25/31] fix unused variables errors --- src/encoders/as5047/AS5047.cpp | 14 +++++------ src/encoders/as5047u/AS5047U.cpp | 42 ++++++++++++++++---------------- src/encoders/as5048a/AS5048A.cpp | 6 ++--- 3 files changed, 31 insertions(+), 31 deletions(-) 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/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/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() }; From c1cba33cad96ebad79f443dd1ecda10b3de02db1 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:45:27 +0200 Subject: [PATCH 26/31] fix github compile issues --- .github/workflows/ccpp.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index a3b75cb..82136db 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -21,8 +21,8 @@ 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 @@ -55,7 +55,7 @@ jobs: 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 + required-libraries: Simple FOC 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 From f65aeaa9f6935dedccea545843959a55812e016a Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:48:38 +0200 Subject: [PATCH 27/31] fix unused variable issue --- src/encoders/aeat8800q24/AEAT8800Q24.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From f0b4fc6455541f0bc04b3a99345e8e14ae0f5a32 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:55:33 +0200 Subject: [PATCH 28/31] exclude some sketches on STM32F103 --- .github/workflows/ccpp.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 82136db..162be4d 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -55,7 +55,8 @@ jobs: 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 mt6816_spi + required-libraries: Simple FOC + 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 From 59faca78fe47545da300d24d891978b830fc0699 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 18:58:34 +0200 Subject: [PATCH 29/31] fix unused variable --- src/encoders/as5048a/PreciseMagneticSensorAS5048A.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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(); } From a6ae49111019512a61a56af880a70500192a0410 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 19:09:42 +0200 Subject: [PATCH 30/31] add badge to README --- README.md | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 2422249..12d3f22 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ ![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.2.3-blue) This library contains an assortment of drivers and supporting code for SimpleFOC. @@ -12,14 +12,15 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un 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's contained +## 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 From 61e02326c6dd2ed2ebb61b0ce8f18ace395a478f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Sat, 15 Oct 2022 19:11:52 +0200 Subject: [PATCH 31/31] fix version number --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 12d3f22..1e40cc4 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ ![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.2.3-blue) +![Release](https://img.shields.io/badge/version-1.0.2-blue) This library contains an assortment of drivers and supporting code for SimpleFOC.