Hello, I’ve been trying to get some angle data from the TMAG5273 sensor on the IOT motor driver board.
Im getting angle data but its very noisy and seems to be in the need of some filtering, I was hoping that i could use the data from this sensor as the input for a closed loop position control of the bldc gimbal motor on the same board but this does not seem to work well with the data coming from the sensor.
Im communicating with it using a GenericSensor object from simpleFOC library and looking at the raw signal data using teleplot on VSCode using the code below. It’s not really cleaned up or anything as im just trying to get a value as a start.
On the image im slowly rotating the motor on said board, the overall trend is what you would expect, but i was hoping that the signal would be cleaner and without the “going back to 0 noise”. On this particular capture the motor is actually plugged out to remove any noise coming from the bldc motor itself.
Is there any recommended code or a “board test” project that i could try? Or is this what you can expect from this sensor?
Best regards, Niklas
#include <Arduino.h>
#include <Wire.h> // Used to establish serial communication on the I2C bus
#include "SparkFun_TMAG5273_Arduino_Library.h" // Used to send and recieve specific information from our sensor
/******************************************************************************
IoT Motor Driver Example
Written By:
Madison Chodikov
Eric Orosel
Company: SparkFun Electronics
Date: September 1 2023
This sketch is a stripped down version of the firmware that is preprogrammed
on the IoT Motor Driver. It is based on the open loop, velocity motor control
example from the SimpleFOC Arduino library.
This sketch will spin the motor based on the button inputs:
- Button 13: Starts/Stops the motor rotation
- Button 14: When spinning, switches the direction of rotation
===============================================================================
Products:
IoT Brushless Motor Driver: https://www.sparkfun.com/products/22132
Repository:
https://github.com/sparkfun/SparkFun_IoT_Brushless_Motor_Driver
===============================================================================
SparkFun code, firmware, and software is released under the MIT
License (http://opensource.org/licenses/MIT).
Distributed as-is; no warranty is given.
******************************************************************************/
#include <Wire.h>
#include <SimpleFOC.h> //http://librarymanager/All#Simple%20FOC
// GPIO
#define auxBtn2 13
#define auxBtn1 14
// driver
#define uh16 16
#define ul17 17
#define vh18 18
#define wh19 19
#define vl23 23
#define wl33 33
#define curSense 32
bool state = true;
// motor driver
BLDCMotor motor = BLDCMotor(1);
BLDCDriver6PWM driver = BLDCDriver6PWM(uh16, ul17, vh18, vl23, wh19, wl33, curSense);
float target_velocity = 0.0;
Commander command = Commander(Serial);
void doTarget(char *cmd) { command.scalar(&target_velocity, cmd); }
void doLimit(char *cmd) { command.scalar(&motor.voltage_limit, cmd); }
// Hall encoder
// Initialize hall-effect sensor
TMAG5273 sensor;
// I2C default address
uint8_t i2cAddress = TMAG5273_I2C_ADDRESS_INITIAL;
// Set constants for setting up device
uint8_t conversionAverage = TMAG5273_X16_CONVERSION;
uint8_t magneticChannel = TMAG5273_X_Y_ENABLE;
uint8_t angleCalculation = TMAG5273_XY_ANGLE_CALCULATION;
// simple FOC TEST mag_sensor
float readMySensor()
{
// read my sensor
// return the angle value in radians in between 0 and 2PI
volatile float reading = sensor.getAngleResult();
return reading;
}
void initMySensor()
{
Wire.begin(21, 22);
if (sensor.begin(i2cAddress, Wire) == true)
{
// {
sensor.begin(i2cAddress, Wire);
sensor.setConvAvg(conversionAverage);
// Choose new angle to calculate from
// Can calculate angles between XYX, YXY, YZY, and XZX
sensor.setMagneticChannel(magneticChannel);
// Enable the angle calculation register
// Can choose between XY, YZ, or XZ priority
sensor.setAngleEn(angleCalculation);
sensor.setMagneticGain(0.5);
}
else
{
Serial.println("could not init mag sensor...");
while (true)
{
;
}
}
}
GenericSensor mySensor(readMySensor, initMySensor);
//////////////////////motor demo stuff///////////////////////////
struct Button
{
const uint8_t PIN;
uint32_t numberKeyPresses;
bool pressed;
};
Button aux1 = {auxBtn1, 0, false};
Button aux2 = {auxBtn2, 0, false};
void IRAM_ATTR isr1()
{
aux1.pressed = true;
target_velocity = target_velocity * (-1);
Serial.println("Changing directions.. ");
}
void IRAM_ATTR isr2()
{
aux2.numberKeyPresses++;
aux2.pressed = true;
if ((aux2.numberKeyPresses % 2) == 0)
{
target_velocity = 0;
Serial.println("Stopping motor.. ");
}
else
{
target_velocity = 2;
motor.enable();
Serial.println("Starting motor.. ");
}
}
void setup()
{
Serial.begin(115200);
Serial.printf("started demo...\n");
delay(5000);
mySensor.init();
// // If the TMAG5273begin is successful (1), then start example
// if (sensor.begin(i2cAddress, Wire) == true)
// {
// Serial.println("Begin");
// // Set the device at 32x average mode
// sensor.setConvAvg(conversionAverage);
// // Choose new angle to calculate from
// // Can calculate angles between XYX, YXY, YZY, and XZX
// sensor.setMagneticChannel(magneticChannel);
// // Enable the angle calculation register
// // Can choose between XY, YZ, or XZ priority
// sensor.setAngleEn(angleCalculation);
// }
// else // Otherwise, infinite loop
// {
// Serial.println("Device failed to setup - Freezing code.");
// while (1)
// ; // Runs forever
// }
// motor demo stuff
driver.voltage_power_supply = 5;
driver.pwm_frequency = 20000;
driver.voltage_limit = 5;
driver.init();
motor.linkDriver(&driver);
// link the motor to the sensor
motor.linkSensor(&mySensor);
motor.voltage_limit = 5;
// motor.controller = MotionControlType::velocity_openloop;
motor.controller = MotionControlType::angle;
// motor.disable();
pinMode(aux1.PIN, INPUT_PULLUP); // Sets pin 14 on the ESP32 as an interrupt
attachInterrupt(aux1.PIN, isr1, FALLING); // Triggers when aux1 is pulled to GND (button pressed)
pinMode(aux2.PIN, INPUT_PULLUP); // Sets pin 13 on the ESP32 as an interrupt
attachInterrupt(aux2.PIN, isr2, FALLING); // Triggers when aux2 is pulled to GND (button pressed)
delay(100);
// velocity PID controller parameters
// default P=0.5 I = 10 D =0
motor.PID_velocity.P = 0.1;
motor.PID_velocity.I = 0;
motor.PID_velocity.D = 0.00;
// jerk control using voltage voltage ramp
// default value is 300 volts per sec ~ 0.3V per millisecond
motor.PID_velocity.output_ramp = 1000;
// velocity low pass filtering
// default 5ms - try different values to see what is the best.
// the lower the less filtered
motor.LPF_velocity.Tf = 0.01;
// angle P controller - default P=20
motor.P_angle.P = 10;
// maximal velocity of the position control
// default 20
motor.velocity_limit = 3;
// default voltage_power_supply
motor.useMonitoring(Serial);
motor.init();
motor.initFOC();
// align encoder and start FOC
}
/////////////////////////////////////////////////////////////////////////
void loop()
{
// user communication
Serial.printf(">mySensorAngle:%f\n", mySensor.getAngle());
// Serial.printf(">TargetAngle:%f\n", target_angle);
mySensor.update();
delay(5);
}
Image from teleplot: