So smoothing is added (or at least I think), as shown below:
#include <Arduino.h>
#include "./hw_config.h"
#include "SimpleFOC.h"
#include "SimpleFOCDrivers.h"
#include "encoders/stm32hwencoder/STM32HWEncoder.h"
#include "encoders/as5047/MagneticSensorAS5047.h"
#include "encoders/smoothing/SmoothingSensor.h"
#define PRODUCT_NAME "SH0002 brushless motor driver"
#define FIRMWARE_VERSION "1.0.0 (2024/11)"
// ==================== MOTOR & DRIVER ====================
// Example motor with 7 pole pairs
BLDCMotor motor(7, 2.30, 312.5, 0.00045);
// 3-PWM driver pins
// PWM_A=PC6, PWM_B=PC7, PWM_C=PC8, enable pin=PC11 (adjust as needed)
BLDCDriver3PWM driver(PC6, PC7, PC8, PC11);
//7——Pole Pairs
HallSensor sensor = HallSensor(PB15, PB12, PA15, 7);// U V W Pole Pairs
SmoothingSensor smooth = SmoothingSensor(sensor, motor);
void doA(){sensor.handleA();}
void doB(){sensor.handleB();}
void doC(){sensor.handleC();}
// inline current sensor instance
// ACS712-05B has the resolution of 0.185mV per Amp
//InlineCurrentSense current_sense = InlineCurrentSense(185.0f, PA3, PC0);
// angle set point variable
float target = 2;
uint32_t loop_ts;
// instantiate the commander
Commander command = Commander(Serial);
void doTarget(char* cmd) { command.scalar(&target, cmd); }
void setup() {
pinMode(PC6, INPUT_PULLUP); // LED_BUILTIN
pinMode(PB11, INPUT_PULLUP);
pinMode(PA3, INPUT_PULLUP);
// initialize sensor hardware
sensor.init();
sensor.enableInterrupts(doA, doB, doC);
// link the motor to the sensor
//motor.linkSensor(&sensor);
motor.linkSensor(&smooth);
// driver config
// power supply voltage [V]
driver.voltage_power_supply = 24;
driver.init();
// link the motor and the driver
motor.linkDriver(&driver);
// link current sense and the driver
//analogReadResolution(12);
//current_sense.linkDriver(&driver);
// aligning voltage [V]
motor.voltage_sensor_align = 6;
// index search velocity [rad/s]
motor.velocity_index_search = 6;
// set motion control loop to be used
motor.controller = MotionControlType::velocity;
// contoller configuration
// default parameters in defaults.h
// velocity PI controller parameters
motor.PID_velocity.P = 0.4f;
motor.PID_velocity.I = 4;
motor.PID_velocity.D = 0;
// default voltage_power_supply
motor.current_limit = 1;
// 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 time constant
motor.LPF_velocity.Tf = 0.01f;
// angle P controller
//motor.P_angle.P = 40;
// maximal velocity of the position control
motor.velocity_limit = 6;
Serial.println(F("Init current sense."));
// current sense init and linking
//current_sense.init();
//motor.linkCurrentSense(¤t_sense);
// use monitoring with serial
Serial.begin(115200);
// comment out if not needed
motor.useMonitoring(Serial);
// initialize motor
motor.init();
// invert phase b gain
//current_sense.gain_b *=-1;
// skip alignment
//current_sense.skip_align = true;
// align sensor and start FOC
motor.initFOC();
// set the inital target value
motor.target = 2;
// add target command T
command.add('T', doTarget, "target angle");
Serial.println(F("Motor ready."));
Serial.println(F("Set the target angle using serial terminal:"));
_delay(1000);
}
void loop() {
// main FOC algorithm function
// the faster you run this function the better
// Arduino UNO loop ~1kHz
// Bluepill loop ~10kHz
motor.loopFOC();
// Motion control function
// velocity, position or voltage (defined in motor.controller)
// this function can be run at much lower frequency than loopFOC() function
// You can also use motor.move() and set the motor.target in the code
motor.move(target);
long now = micros();
if (now - loop_ts > 1000000) {
Serial.print("T:");
Serial.print(target, 2);
Serial.print(" A:");
Serial.print(motor.shaft_angle, 2);
Serial.print(" V:");
Serial.print(motor.shaft_velocity, 2);
Serial.print(" D:");
float diff = fabs(motor.shaft_velocity-target);
Serial.print(diff, 2);
Serial.print(" %:");
float per = target!=0.0f?diff/target*100.0f:0.0f;
Serial.print(per, 1);
Serial.print(" I:");
Serial.println();
loop_ts = now;
}
// function intended to be used with serial plotter to monitor motor variables
// significantly slowing the execution down!!!!
// motor.monitor();
// user communication
command.run();
}
Still no movement from velocity but a weird thing happened in torque mode.
I force the shaft to move and after couple of rotation it started rotating (and vibrating).
Putting the torque back to 0 and to a value again got me in the same state:
Rotate the shaft by hand, forcing on the motor and getting the shaft to rotate.
Could it be related to the shaft not moving in velocity mode ?
And so to the tunning ?
Anyways I will try to tune my settings and see how it goes.
Update:
I put the motor in velocity mode, increased the target slowly and tried to help the shaft rotate to initiate the movement and I’m finnaly getting something !
100.000
T:100.00 A:715.24 V:0.00 D:100.00 %:100.0 I:
T:100.00 A:715.24 V:0.00 D:100.00 %:100.0 I:
T:100.00 A:823.99 V:100.54 D:0.54 %:0.5 I:
T:100.00 A:949.51 V:99.74 D:0.26 %:0.3 I:
T:100.00 A:1075.02 V:100.05 D:0.05 %:0.0 I:
T:100.00 A:1200.62 V:99.96 D:0.04 %:0.0 I:
T:100.00 A:1326.03 V:99.89 D:0.11 %:0.1 I:
T:100.00 A:1451.58 V:100.26 D:0.26 %:0.3 I:
T:100.00 A:1577.06 V:100.09 D:0.09 %:0.1 I:
T:100.00 A:1702.61 V:99.59 D:0.41 %:0.4 I:
T:100.00 A:1828.11 V:99.86 D:0.14 %:0.1 I:
T:100.00 A:1953.76 V:99.82 D:0.18 %:0.2 I:
T:100.00 A:2079.33 V:99.78 D:0.22 %:0.2 I:
T:100.00 A:2204.77 V:99.83 D:0.17 %:0.2 I:
T:100.00 A:2330.36 V:100.05 D:0.05 %:0.0 I:
T:100.00 A:2455.83 V:100.11 D:0.11 %:0.1 I:
T:100.00 A:2581.48 V:99.96 D:0.04 %:0.0 I:
T:100.00 A:2707.11 V:99.82 D:0.18 %:0.2 I:
T:100.00 A:2832.74 V:99.73 D:0.27 %:0.3 I:
T:100.00 A:2958.37 V:99.62 D:0.38 %:0.4 I:
T:100.00 A:3084.00 V:99.52 D:0.48 %:0.5 I:
T:100.00 A:3209.59 V:99.61 D:0.39 %:0.4 I:
It does vibrate a lot though, which suggests that the tunning is off but it’s at least a beginning !
I will continue my investigation 
