From cec8df067bda7d2537fb0f679dd90266b28e0a9b Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 10 Feb 2026 08:25:55 +0100 Subject: [PATCH 1/4] fist implementation of the custom motion control type --- src/common/base_classes/FOCMotor.cpp | 18 +++++++++++++----- src/common/base_classes/FOCMotor.h | 15 ++++++++++++++- src/communication/Commander.cpp | 7 +++++-- 3 files changed, 32 insertions(+), 8 deletions(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index 837bf210..c9861fe5 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -584,7 +584,8 @@ void FOCMotor::loopFOC() { if (sensor) sensor->update(); // if disabled do nothing - if(!enabled) return; + // or if the motor is not ready (e.g. failed to calibrate or not calibrated yet) do nothing + if(!enabled || motor_status != FOCMotorStatus::motor_ready) return; // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) @@ -691,7 +692,8 @@ void FOCMotor::move(float new_target) { } // if disabled do nothing - if(!enabled) return; + // and if + if(!enabled || motor_status != FOCMotorStatus::motor_ready) return; // upgrade the current based voltage limit @@ -745,6 +747,12 @@ void FOCMotor::move(float new_target) { // returned values correspond to the voltage_limit and current_limit current_sp = angleOpenloop(shaft_angle_sp); break; + case MotionControlType::custom: + // custom control - user provides the function that calculates the current_sp + // based on the target value and the motor state + // user makes sure to use it with appropriate torque control mode + if(customMotionControlCallback) + current_sp = customMotionControlCallback(this, target); } } @@ -768,8 +776,8 @@ int FOCMotor::initFOC() { SIMPLEFOC_MOTOR_DEBUG("No sensor."); if ((controller == MotionControlType::angle_openloop || controller == MotionControlType::velocity_openloop)){ exit_flag = 1; - SIMPLEFOC_MOTOR_ERROR("Openloop only!"); }else{ + SIMPLEFOC_MOTOR_ERROR("Only openloop allowed!"); exit_flag = 0; // no FOC without sensor } } @@ -781,13 +789,13 @@ int FOCMotor::initFOC() { if(current_sense){ if (!current_sense->initialized) { motor_status = FOCMotorStatus::motor_calib_failed; - SIMPLEFOC_MOTOR_ERROR("Init FOC error, current sense not init"); + SIMPLEFOC_MOTOR_ERROR("Current sense not init!"); exit_flag = 0; }else{ exit_flag *= alignCurrentSense(); } } - else { SIMPLEFOC_MOTOR_ERROR("No current sense"); } + else { SIMPLEFOC_MOTOR_ERROR("No current sense."); } } if(exit_flag){ diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 029c69fb..7f9aaba2 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -48,7 +48,8 @@ enum MotionControlType : uint8_t { angle = 0x02, //!< Position/angle motion control velocity_openloop = 0x03, angle_openloop = 0x04, - angle_nocascade = 0x05 //!< Position/angle motion control without velocity cascade + angle_nocascade = 0x05, //!< Position/angle motion control without velocity cascade + custom = 0x06 //!< Custom control method - control method added by user }; /** @@ -367,6 +368,16 @@ class FOCMotor */ float angleOpenloop(float target_angle); + + /** + * Function setting a custom motion control method defined by the user + * @note the custom control method has to be defined by the user and should follow the signature: float controlMethod(FOCMotor* motor, float target) + * @param controlMethod - pointer to the custom control method function defined by the user + */ + void setCustomMotionControlMethod(float (*controlMethod)(FOCMotor* motor, float target)){ + customMotionControlCallback = controlMethod; + } + protected: /** @@ -410,6 +421,8 @@ class FOCMotor // open loop variables uint32_t open_loop_timestamp; + // function pointer for custom control method + float (*customMotionControlCallback)(FOCMotor* motor, float target) = nullptr; }; diff --git a/src/communication/Commander.cpp b/src/communication/Commander.cpp index ba8ae5c2..487add9c 100644 --- a/src/communication/Commander.cpp +++ b/src/communication/Commander.cpp @@ -439,7 +439,7 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){ break; default: // change control type - if(!GET && value >= 0 && (int)value < 6) // if set command + if(!GET && value >= 0 && (int)value < 7) // if set command motor->updateMotionControlType((MotionControlType)value); // update motion control type switch(motor->controller){ case MotionControlType::torque: @@ -460,6 +460,9 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){ case MotionControlType::angle_nocascade: println(F("angle nocascade")); break; + case MotionControlType::custom: + println(F("custom")); + break; } break; } @@ -467,7 +470,7 @@ void Commander::motion(FOCMotor* motor, char* user_cmd, char* separator){ case CMD_TORQUE_TYPE: // change control type printVerbose(F("Torque: ")); - if(!GET && (int8_t)value >= 0 && (int8_t)value < 4)// if set command + if(!GET && (int8_t)value >= 0 && (int8_t)value < 4) // if set command motor->updateTorqueControlType((TorqueControlType)value); // update torque control type switch(motor->torque_controller){ case TorqueControlType::voltage: From 18fbfe6253f3c50bbeef863619ade580ec694921 Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 10 Feb 2026 08:57:48 +0100 Subject: [PATCH 2/4] added an example --- .github/workflows/arduino.yml | 3 +- .github/workflows/stm32.yml | 27 +++-- .../custom_motion_control.ino | 109 ++++++++++++++++++ src/common/base_classes/FOCMotor.cpp | 3 + src/common/pid.cpp | 6 +- src/common/pid.h | 2 +- 6 files changed, 136 insertions(+), 14 deletions(-) create mode 100644 examples/motion_control/custom_motion_control/custom_motion_control.ino diff --git a/.github/workflows/arduino.yml b/.github/workflows/arduino.yml index 37d12ab6..a5da14e2 100644 --- a/.github/workflows/arduino.yml +++ b/.github/workflows/arduino.yml @@ -33,7 +33,8 @@ jobs: esp32_current_control_low_side, stm32_spi_alt_example, esp32_spi_alt_example, B_G431B_ESC1, odrive_example_spi, odrive_example_encoder, single_full_control_example, double_full_control_example, stm32_current_control_low_side, open_loop_velocity_6pwm, - efr32_hall_sensor_velocity_6pwm, efr32_open_loop_velocity_6pwm, efr32_torque_velocity_6pwm + efr32_hall_sensor_velocity_6pwm, efr32_open_loop_velocity_6pwm, efr32_torque_velocity_6pwm, + custom_motion_control - arduino-boards-fqbn: arduino:sam:arduino_due_x # arduino due - one full example sketch-names: single_full_control_example.ino diff --git a/.github/workflows/stm32.yml b/.github/workflows/stm32.yml index 52b5cc94..67b07cc3 100644 --- a/.github/workflows/stm32.yml +++ b/.github/workflows/stm32.yml @@ -16,12 +16,14 @@ jobs: strategy: matrix: arduino-boards-fqbn: - - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill - - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo - - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg - - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive - - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 - - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 + - STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # stm32 bluepill + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # stm32 nucleo + - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # stm32 nucleo f746zg + - STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX # stm32f405 - odrive + - STMicroelectronics:stm32:GenL4:pnum=GENERIC_L475RGTX # stm32l475 + - STMicroelectronics:stm32:Disco:pnum=B_G431B_ESC1 # B-G431-ESC1 + - STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_H723ZG # stm32h7 + - STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_G431RB # stm32g4 include: - arduino-boards-fqbn: STMicroelectronics:stm32:GenF1:pnum=BLUEPILL_F103C8 # bluepill - hs examples @@ -45,12 +47,19 @@ jobs: - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_F411RE # nucleo one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino, custom_motion_control.ino - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_F746ZG # nucleo f7 one full example platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json - sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino - + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino, custom_motion_control.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_144:pnum=NUCLEO_H723ZG # nucleo h7 one full example + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino, custom_motion_control.ino + + - arduino-boards-fqbn: STMicroelectronics:stm32:Nucleo_64:pnum=NUCLEO_G431RB # stm32g4 one full example + platform-url: https://github.com/stm32duino/BoardManagerFiles/raw/main/package_stmicroelectronics_index.json + sketch-names: single_full_control_example.ino, stm32_spi_alt_example.ino, double_full_control_example.ino, stm32_current_control_low_side.ino, custom_motion_control.ino # Do not cancel all jobs / architectures if one job fails fail-fast: false diff --git a/examples/motion_control/custom_motion_control/custom_motion_control.ino b/examples/motion_control/custom_motion_control/custom_motion_control.ino new file mode 100644 index 00000000..c0e84367 --- /dev/null +++ b/examples/motion_control/custom_motion_control/custom_motion_control.ino @@ -0,0 +1,109 @@ + +#include +#include +#include "current_sense/hardware_specific/stm32/stm32_mcu.h" + + +// BLDC motor & driver instance +BLDCMotor motor = BLDCMotor(7); +BLDCDriver3PWM driver = BLDCDriver3PWM(D6, D10, D5, D8); + +// encoder instance +MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, D4); + +// inline current sensor instance +// INA240A1 (gain 20V/V) and 5mOhm shunt resistor +LowsideCurrentSense current_sense = LowsideCurrentSense(0.005, 20.0f, A0, _NC, A3); + +// commander communication instance +Commander command = Commander(Serial); +// void doMotion(char* cmd){ command.motion(&motor, cmd); } +void doMotor(char* cmd){ command.motor(&motor, cmd); } + + +// custom PID controller instance for the custom control method +// P controller with gain of 1.0f, no integral or derivative gain +PIDController custom_PID = PIDController(1.0f, 0, 0); +// custom motion control method +float positionPControl(FOCMotor* motor, float target){ + // simple proportional position control + float error = target - motor->shaft_angle; + // set the PID output limit to the motor current limit + custom_PID.limit = motor->current_limit; + return custom_PID(error); // return current command based on the error +} + +// optional add the PID to command to be able to tune it in runtime +void doPID(char* cmd){ command.pid(&custom_PID, cmd); } + +void setup() { + // use monitoring with serial + Serial.begin(115200); + // enable more verbose output for debugging + // comment out if not needed + SimpleFOCDebug::enable(&Serial); + + // initialize sensor hardware + sensor.init(&SPI); + motor.linkSensor(&sensor); + + // driver config + // power supply voltage [V] + driver.voltage_power_supply = 20; + driver.init(); + // link driver + motor.linkDriver(&driver); + // link current sense and the driver + current_sense.linkDriver(&driver); + + // set the custom control method as the position P control + motor.setCustomMotionControlMethod(positionPControl); + // set control loop type to be used + motor.controller = MotionControlType::custom; + // set the torque control type to voltage control (default is voltage control) + motor.torque_controller = TorqueControlType::foc_current; + + // velocity low pass filtering time constant + motor.LPF_velocity.Tf = 0.01f; + + // angle loop controller + motor.P_angle.P = 20; + // angle loop velocity limit + motor.velocity_limit = 20; + motor.voltage_sensor_align = 1.0f; + + // comment out if not needed + motor.useMonitoring(Serial); + motor.monitor_downsample = 0; // disable intially + motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle + + // current sense init and linking + current_sense.init(); + motor.linkCurrentSense(¤t_sense); + + // initialise motor + motor.init(); + // align encoder and start FOC + motor.initFOC(); + + // subscribe motor to the commander + //command.add('T', doMotion, "motion control"); // a bit less resouce intensive + command.add('M', doMotor, "motor"); + command.add('C', doPID, "custom PID"); + + _delay(1000); +} + +void loop() { + // iterative setting FOC phase voltage + motor.loopFOC(); + + // iterative function setting the outter loop target + motor.move(); + + // // motor monitoring + motor.monitor(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index c9861fe5..4247cf6c 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -521,6 +521,8 @@ void FOCMotor::updateMotionControlType(MotionControlType new_motion_controller) target = 0; break; default: + // if torque control set target to zero + target = 0; break; } @@ -753,6 +755,7 @@ void FOCMotor::move(float new_target) { // user makes sure to use it with appropriate torque control mode if(customMotionControlCallback) current_sp = customMotionControlCallback(this, target); + break; } } diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 7489554d..3bf3b491 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -37,7 +37,7 @@ float PIDController::operator() (float error){ // u_ik = u_ik_1 + I*Ts/2*(ek + ek_1) float integral = integral_prev + I*dt*0.5f*(error + error_prev); // antiwindup - limit the output - integral = _constrain(integral, -limit, limit); + if(_isset(limit)) integral = _constrain(integral, -limit, limit); // Discrete derivation // u_dk = D(ek - ek_1)/Ts float derivative = D*(error - error_prev)/dt; @@ -45,10 +45,10 @@ float PIDController::operator() (float error){ // sum all the components float output = proportional + integral + derivative; // antiwindup - limit the output variable - output = _constrain(output, -limit, limit); + if(_isset(limit)) output = _constrain(output, -limit, limit); // if output ramp defined - if(output_ramp > 0){ + if(_isset(output_ramp) && output_ramp > 0){ // limit the acceleration by ramping the output float output_rate = (output - output_prev)/dt; if (output_rate > output_ramp) diff --git a/src/common/pid.h b/src/common/pid.h index c171e30d..20e11237 100644 --- a/src/common/pid.h +++ b/src/common/pid.h @@ -25,7 +25,7 @@ class PIDController * @note Sampling time can be changed dynamically as well by modifying the * variable Ts in runtime. */ - PIDController(float P, float I, float D, float ramp, float limit, float sampling_time = NOT_SET); + PIDController(float P, float I, float D, float ramp = NOT_SET, float limit = NOT_SET, float sampling_time = NOT_SET); ~PIDController() = default; float operator() (float error); From b9fa9ee8b38821c1dd2ee79344d47661ee9512c3 Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 10 Feb 2026 09:08:35 +0100 Subject: [PATCH 3/4] remove unnecessary things --- .../custom_motion_control.ino | 23 ++++++------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/examples/motion_control/custom_motion_control/custom_motion_control.ino b/examples/motion_control/custom_motion_control/custom_motion_control.ino index c0e84367..1a621dd8 100644 --- a/examples/motion_control/custom_motion_control/custom_motion_control.ino +++ b/examples/motion_control/custom_motion_control/custom_motion_control.ino @@ -44,7 +44,7 @@ void setup() { SimpleFOCDebug::enable(&Serial); // initialize sensor hardware - sensor.init(&SPI); + sensor.init(); motor.linkSensor(&sensor); // driver config @@ -56,27 +56,23 @@ void setup() { // link current sense and the driver current_sense.linkDriver(&driver); - // set the custom control method as the position P control + // set the custom control method motor.setCustomMotionControlMethod(positionPControl); // set control loop type to be used motor.controller = MotionControlType::custom; // set the torque control type to voltage control (default is voltage control) motor.torque_controller = TorqueControlType::foc_current; - // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01f; - - // angle loop controller - motor.P_angle.P = 20; - // angle loop velocity limit - motor.velocity_limit = 20; - motor.voltage_sensor_align = 1.0f; - // comment out if not needed motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle + // subscribe motor to the commander + //command.add('T', doMotion, "motion control"); // a bit less resouce intensive + command.add('M', doMotor, "motor"); + command.add('C', doPID, "custom PID"); + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -86,11 +82,6 @@ void setup() { // align encoder and start FOC motor.initFOC(); - // subscribe motor to the commander - //command.add('T', doMotion, "motion control"); // a bit less resouce intensive - command.add('M', doMotor, "motor"); - command.add('C', doPID, "custom PID"); - _delay(1000); } From 9b0c1330e6a9a2fbe3083b7f2e9684c57d6c8298 Mon Sep 17 00:00:00 2001 From: gospar Date: Tue, 10 Feb 2026 09:18:40 +0100 Subject: [PATCH 4/4] rename set to link --- .../custom_motion_control/custom_motion_control.ino | 2 +- src/common/base_classes/FOCMotor.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/motion_control/custom_motion_control/custom_motion_control.ino b/examples/motion_control/custom_motion_control/custom_motion_control.ino index 1a621dd8..6e4c9d65 100644 --- a/examples/motion_control/custom_motion_control/custom_motion_control.ino +++ b/examples/motion_control/custom_motion_control/custom_motion_control.ino @@ -57,7 +57,7 @@ void setup() { current_sense.linkDriver(&driver); // set the custom control method - motor.setCustomMotionControlMethod(positionPControl); + motor.linkCustomMotionControl(positionPControl); // set control loop type to be used motor.controller = MotionControlType::custom; // set the torque control type to voltage control (default is voltage control) diff --git a/src/common/base_classes/FOCMotor.h b/src/common/base_classes/FOCMotor.h index 7f9aaba2..39475481 100644 --- a/src/common/base_classes/FOCMotor.h +++ b/src/common/base_classes/FOCMotor.h @@ -374,7 +374,7 @@ class FOCMotor * @note the custom control method has to be defined by the user and should follow the signature: float controlMethod(FOCMotor* motor, float target) * @param controlMethod - pointer to the custom control method function defined by the user */ - void setCustomMotionControlMethod(float (*controlMethod)(FOCMotor* motor, float target)){ + void linkCustomMotionControl(float (*controlMethod)(FOCMotor* motor, float target)){ customMotionControlCallback = controlMethod; }