On this page
Motion control implementation v2.4+
BLDC motors Stepper motors
Motion control is the outer control loop in the SimpleFOClibrary, executed within move(). It converts user commands (position, velocity, or torque targets) into a current setpoint (current_sp) that is passed to the torque control layer in loopFOC().
The library implements 7 motion control strategies:
enum MotionControlType : uint8_t {
torque = 0x00, // Direct torque/current control
velocity = 0x01, // Velocity motion control
angle = 0x02, // Position control (cascade)
velocity_openloop = 0x03, // Open-loop velocity
angle_openloop = 0x04, // Open-loop position
angle_nocascade = 0x05, // Position control (direct)
custom = 0x06 // User-defined control
};
Selection:
motor.controller = MotionControlType::velocity; // Can be changed in real-time
The move() function
Motion control is executed in the move() function, which should be called iteratively in the main loop. The function accepts an optional target parameter; if omitted, it uses motor.target.
void loop() {
motor.loopFOC(); // Fast: torque control
motor.move(); // Slower: motion control
// or
motor.move(new_target); // Override motor.target
}
The real-time motion control is executed inside the move() function. This function executes one of the control loops based on the controller variable. The parameter new_target is the target value to be set for the control loop. The new_target value is optional; if not set, the motion control will use the motor.target variable.
Here is a simplified view of the implementation:
// Iterative function running outer loop of the FOC algorithm
// Behavior of this function is determined by the motor.controller variable
// - needs to be called iteratively, it is an asynchronous function
// - if target is not set it uses motor.target value
void FOCMotor::move(float new_target) {
// set internal target variable
if(_isset(new_target)) target = new_target;
// downsampling (optional)
if(motion_cnt++ < motion_downsample) return;
motion_cnt = 0;
// read sensor values (except for open loop modes)
if(controller != MotionControlType::angle_openloop &&
controller != MotionControlType::velocity_openloop) {
shaft_angle = shaftAngle();
shaft_velocity = shaftVelocity();
}
// if disabled or not ready, do nothing
if(!enabled || motor_status != FOCMotorStatus::motor_ready) return;
// choose control loop
switch (controller) {
case MotionControlType::torque:
current_sp = target;
break;
case MotionControlType::angle_nocascade:
// angle set point
shaft_angle_sp = target;
// calculate the torque command directly from position error
current_sp = P_angle(shaft_angle_sp - LPF_angle(shaft_angle));
break;
case MotionControlType::angle:
// angle set point
shaft_angle_sp = target;
// calculate velocity set point
shaft_velocity_sp = feed_forward_velocity + P_angle(shaft_angle_sp - LPF_angle(shaft_angle));
shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
// calculate the torque command
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity);
break;
case MotionControlType::velocity:
// velocity set point
shaft_velocity_sp = target;
// calculate the torque command
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity);
break;
case MotionControlType::velocity_openloop:
shaft_velocity_sp = target;
current_sp = velocityOpenloop(shaft_velocity_sp);
break;
case MotionControlType::angle_openloop:
shaft_angle_sp = target;
current_sp = angleOpenloop(shaft_angle_sp);
break;
case MotionControlType::custom:
// custom control - user provides the callback function
if(customMotionControlCallback)
current_sp = customMotionControlCallback(this, target);
break;
}
}
The move() function is typically called at the same frequency as loopFOC(), but it can be downsampled using the motion_downsample variable to reduce computational load. This can allow for more complex motion control algorithms that do not need to run at the full speed of the torque control loop.
\[f_{MC} = \frac{f_{TC}}{\texttt{motion_downsample} }\]
Torque Mode
Type: MotionControlType::torque
BLDC motors Stepper motors
Direct torque/current control - the target value is passed directly as the current setpoint to the torque controller.
case MotionControlType::torque:
current_sp = target;
break;
The actual torque control strategy (voltage, DC current, FOC current, estimated current) is determined by motor.torque_controller.
Detailed implementation
For detailed information about torque control modes, see the torque control implementation page.
Torque control API documentation
Velocity Control
Type: MotionControlType::velocity
BLDC motors Stepper motors
Closed-loop velocity control using a PID controller to calculate the current setpoint from velocity error.
Control Flow
case MotionControlType::velocity:
shaft_velocity_sp = target;
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity);
break;
Velocity Controller
The PID_velocity controller is a PIDController object. Also note the low-pass filtering of the velocity measurement before calculating the error.
motor.PID_velocity.P = 0.2;
motor.PID_velocity.I = 20.0;
motor.PID_velocity.D = 0.0;
motor.LPF_velocity.Tf = 0.01; // 10ms low-pass filter time constant
Velocity control API documentation PID controller implementation
Additional Features
Velocity low-pass filtering:
motor.LPF_velocity.Tf = 0.01; // 10ms low-pass filter time constant
Low-pass filter implementation
Position Control (Cascade Mode)
Type: MotionControlType::angle
BLDC motors Stepper motors
Cascaded position control: P controller generates velocity setpoint, then velocity PID calculates current setpoint.
Control Flow
case MotionControlType::angle:
shaft_angle_sp = target;
// P controller calculates velocity setpoint
shaft_velocity_sp = feed_forward_velocity +
P_angle(shaft_angle_sp - LPF_angle(shaft_angle));
// Constrain velocity
shaft_velocity_sp = _constrain(shaft_velocity_sp, -velocity_limit, velocity_limit);
// Velocity PID calculates current setpoint
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity);
break;
Position Controller
The P_angle controller is a PIDController object (though typically only P gain is used):
motor.P_angle.P = 20.0; // Proportional gain
motor.P_angle.I = 0.0; // Usually zero
motor.P_angle.D = 0.0; // Usually zero
Angle control API documentation PID controller implementation
Additional Features
Angle low-pass filtering:
motor.LPF_angle.Tf = 0.0; // Usually disabled (0)
Low-pass filter implementation
Feed-forward velocity:
motor.feed_forward_velocity = 0.5; // [rad/s]
Velocity limiting:
motor.velocity_limit = 10.0; // [rad/s]
Position Control (Non-Cascade Mode)
Type: MotionControlType::angle_nocascade
BLDC motors Stepper motors
Direct position-to-torque control without velocity loop intermediary - P controller directly calculates current setpoint.
Control Flow
case MotionControlType::angle_nocascade:
shaft_angle_sp = target;
current_sp = P_angle(shaft_angle_sp - LPF_angle(shaft_angle));
break;
Position Controller
The P_angle controller is a PIDController object (though typically only P gain is used):
motor.P_angle.P = 20.0; // Proportional gain
motor.P_angle.I = 0.0; // Usually very low or zero
motor.P_angle.D = 0.0; // Usually zero
Non-cascade angle control API documentation PID controller implementation
Additional Features
Angle low-pass filtering:
motor.LPF_angle.Tf = 0.0; // Usually disabled (0)
Low-pass filter implementation
Open-Loop Velocity Control
Type: MotionControlType::velocity_openloop
BLDC motors Stepper motors
Open-loop velocity control without sensor feedback - generates rotating field at target velocity.
Control Flow
case MotionControlType::velocity_openloop:
shaft_velocity_sp = target;
current_sp = velocityOpenloop(shaft_velocity_sp);
break;
The velocityOpenloop() function integrates velocity to calculate angle:
float FOCMotor::velocityOpenloop(float target_velocity) {
unsigned long now_us = _micros();
float Ts = (now_us - open_loop_timestamp) * 1e-6f;
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
shaft_angle = _normalizeAngle(shaft_angle + target_velocity * Ts);
shaft_velocity = target_velocity;
open_loop_timestamp = now_us;
return current_sp;
}
Open-loop velocity control API documentation
Open-Loop Position Control
Type: MotionControlType::angle_openloop
BLDC motors Stepper motors
Open-loop position control - sets electrical angle directly without sensor feedback.
Control Flow
case MotionControlType::angle_openloop:
shaft_angle_sp = target;
current_sp = angleOpenloop(shaft_angle_sp);
break;
The angleOpenloop() function sets the angle and applies velocity ramping:
float FOCMotor::angleOpenloop(float target_angle) {
unsigned long now_us = _micros();
float Ts = (now_us - open_loop_timestamp) * 1e-6f;
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
// Calculate angle difference
float angle_diff = _normalizeAngle(target_angle - shaft_angle);
// Apply velocity limit
if(abs(angle_diff) > velocity_limit * Ts) {
angle_diff = _sign(angle_diff) * velocity_limit * Ts;
}
shaft_angle = _normalizeAngle(shaft_angle + angle_diff);
open_loop_timestamp = now_us;
return current_sp;
}
Open-loop position control API documentation
Custom Control
Type: MotionControlType::custom
BLDC motors Stepper motors
User-defined control algorithm via callback function.
Control Flow
case MotionControlType::custom:
if(customMotionControlCallback)
current_sp = customMotionControlCallback(this, target);
break;
Usage
Set the callback function pointer:
// Define custom control function
float myCustomControl(FOCMotor* motor, float target) {
// Your control algorithm here
// Access motor state: motor->shaft_velocity, motor->shaft_angle, etc.
// Return current setpoint
return calculated_current_sp;
}
// Register callback
motor.customMotionControlCallback = myCustomControl;
motor.controller = MotionControlType::custom;
Custom control API documentation