Hello forum, first post here!
Hopefully this is in the right location.
First of all, big thanks for the simple FOC library and this community here! This would have been extremely challenging from scratch without it. I’ve learned so much and have been lurking for some time now without creating an account until now.
I have successfully create a working setup that can control Angle very rapidly without much bounce back or resonance.
I’m using a 7pair pole low cogging torque gimbal motor with an STM32G431RB Nucleo control board.
12V supply for the motor, using a STSPIN830 shield with Low Side Current sensing on 3x 330ohm shunt resistors. Low side current sensing is working well and I can precisely position the motor with FOC_Current and Angle.
The encoder is a 14bit magnetic spi encoder AS5047P and I’m using it in SPI mode.
After tuning like a crazed mad man it’s responding very well to commands in angle mode. Snapping to target position very quickly and precisely.
I’m developing in VSC with PlatformIO and I have been using the SimpleFOC studio for tuning (life saver).
Basic project description:
I’m developing a knob control interface. This will be receiving a string of values from an external source and then I’d like the motor to track the positions being received. When it’s not receiving values and updating positions I’d like it to be moveable by hand.
I’m a fairly novice programmer but I’m learning more everyday.
Sending a fast stream of changing values doesn’t seem to be handled well without me doing some unknown processing of the stream (I may need to filter or process them in some way?).
Main Question
Basically, I don’t know if the angle mode is best for this or if I’d have better luck chasing angles with torque mode and an algorithm that compares target value with motor.shaft_angle and chases that with torque mode?
I tried to do a simple set of for loops (see code) to have the have it update the target value but I’m not sure how to handle the constant change of input data (target values) and have the motor track the constant input of target values. I’d like to be able to send a sine wave of data points and have the knob move like a sine wave for example.
If I replace the for loops with motor.move(); and send an angle command it tracks beautifully. With the for loop I just get random spinning and oscillation. I don’t know that I have thought through how to properly handle a constantly changing input. Just switching gears to developing my application and trying to make sure I’m approaching this the best way.
Any thoughts or input is appreciated!
#include <SimpleFOC.h>
// MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs)
// config - SPI config
// cs - SPI chip select pin
MagneticSensorSPI sensor = MagneticSensorSPI(AS5047_SPI, PB6);
// BLDC motor & driver instance
// float target_current = 0;
BLDCMotor motor = BLDCMotor(7, 5, 255);
// BLDCDriver3PWM(pwmA, pwmB, pwmC, (en optional))
BLDCDriver3PWM driver = BLDCDriver3PWM(PA8, PA9, PA10, PB13, PB14, PB15);
LowsideCurrentSense current_sense = LowsideCurrentSense(0.33, 1.53, PA1, PB1, PB0); // IHM16M1 manual says op-amp gain is 1.53
// include commander interface
Commander command = Commander(Serial);
bool motorCommandReceived = false;
float previousTarget = 0;
const float stepSize = 4.71 / 128;
void doMotor(char *cmd)
{
command.motor(&motor, cmd);
motorCommandReceived = true; // Set the flag when the command is received
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV6;
RCC_OscInitStruct.PLL.PLLN = 85;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV8;
RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
{
Error_Handler();
}
/** Enables the Clock Security System
*/
HAL_RCC_EnableCSS();
}
void setup()
{
// monitoring port
Serial.begin(115200);
// enable more verbose output for debugging
// comment out if not needed
SimpleFOCDebug::enable(&Serial);
sensor.clock_speed = 10000000;
// initialise magnetic sensor hardware
sensor.init();
Serial.println("Sensor ready");
_delay(1000);
// link the motor to the sensor
motor.linkSensor(&sensor);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 12;
// driver.init();
// driver init
if (!driver.init())
{
Serial.println("Driver init failed!");
return;
}
// link driver
motor.linkDriver(&driver);
// link driver to cs
current_sense.linkDriver(&driver);
current_sense.init();
// current sense init hardware
if (!current_sense.init())
{
Serial.println("Current sense init failed!");
return;
}
// link the current sense to the motor
motor.linkCurrentSense(¤t_sense);
// aligning current sense
current_sense.gain_a *= -1;
current_sense.gain_b *= -1;
current_sense.gain_c *= -1;
// current_sense.skip_align = true;
// aligning voltage
motor.voltage_sensor_align = 5;
// control loop type and torque mode
motor.torque_controller = TorqueControlType::foc_current;
motor.controller = MotionControlType::angle;
motor.motion_downsample = 100.0;
// velocity loop PID
motor.PID_velocity.P = 0.09;
motor.PID_velocity.I = 0.5;
motor.PID_velocity.D = 0.001;
motor.PID_velocity.output_ramp = 10000.0;
motor.PID_velocity.limit = 18.0;
// Low pass filtering time constant
motor.LPF_velocity.Tf = 0.2;
// angle loop PID
motor.P_angle.P = 20.0;
motor.P_angle.I = 0.0;
motor.P_angle.D = 3.4;
motor.P_angle.output_ramp = 10000.0;
motor.P_angle.limit = 30.0;
// Low pass filtering time constant
motor.LPF_angle.Tf = 0.2;
// current q loop PID
motor.PID_current_q.P = 4.0;
motor.PID_current_q.I = 0.0;
motor.PID_current_q.D = 0.001;
motor.PID_current_q.output_ramp = 5000.0;
motor.PID_current_q.limit = 2.0;
// Low pass filtering time constant
motor.LPF_current_q.Tf = 2.0;
// current d loop PID
motor.PID_current_d.P = 4.0;
motor.PID_current_d.I = 0.0;
motor.PID_current_d.D = 0.001;
motor.PID_current_d.output_ramp = 5000.0;
motor.PID_current_d.limit = 2.0;
// Low pass filtering time constant
motor.LPF_current_d.Tf = 2.0;
// Limits
motor.velocity_limit = 30.0;
motor.voltage_limit = 12.0;
motor.current_limit = 1.0;
// sensor zero offset - home position
motor.sensor_offset = 0.0;
// general settings
// motor phase resistance
motor.phase_resistance = 6.211;
// pwm modulation settings
motor.foc_modulation = FOCModulationType::SinePWM;
motor.modulation_centered = 1.0;
motor.useMonitoring(Serial);
// motor.monitor_downsample = 1; // set downsampling can be even more > 100
motor.monitor_variables = _MON_CURR_Q | _MON_CURR_D | _MON_TARGET | _MON_VEL | _MON_ANGLE; // set monitoring of d and q currents
// initialize motor
if (!motor.init())
{
Serial.println("Motor init failed!");
return;
}
// align sensor and start FOC
if (!motor.initFOC())
{
Serial.println("FOC init failed!");
return;
}
// set the initial motor target
motor.target = 0; // Volts?
// add target command M
command.add('M', doMotor, "Motor");
// add target command T
// command.add('T', doTarget, "target current");
Serial.println(F("Motor ready."));
Serial.println(F("Set the angle target using serial terminal and command M:"));
_delay(1000);
}
void loop()
{
motor.loopFOC();
// Cycle the motor target from 0 to 127 steps
for (int i = 0; i <= 127; i++)
{
motor.target = i * stepSize;
motor.move();
_delay(100); // Adjust the delay as needed
}
// Cycle the motor target from 127 back to 0 steps
for (int i = 127; i >= 0; i--)
{
motor.target = i * stepSize;
motor.move();
_delay(100); // Adjust the delay as needed
}
// real-time monitoring calls
motor.monitor();
// real-time commander calls
command.run();
}
Here’s a photo of my test setup.
I modified a model I found here to fit my motor and encoder.
