Servo Driver (PWM Motor)

The Servo library provides a complete motor control system for closed-loop servo applications. It implements a hierarchical control architecture with position and velocity control loops, digital filtering, and motor current feedback-based stall detection. The system is designed to run at 100 Hz but can operate at variable cycle times.

Hardware Requirements

• Quadrature encoder on motor shaft (16-bit counter typical)
• PWM motor driver with 2 output channels for bidirectional control
• Current sensor for motor feedback
• Supply voltage measurement capability

Architecture

Class Hierarchy

Servo
    ↓
    └── PWMMotor
            ↓
            ├── PWMOutput (interface)
            ├── PWMQdecInput (interface)
            └── BiquadFilter (template utility)

Key Components

1. Servo (Main Controller)

The top-level servo controller that implements a cascaded control architecture:

• Position Loop: Converts position error to velocity setpoint using proportional control
• Velocity Loop: Implements PID control (with integrator anti-windup) to produce motor duty cycle
• State Estimation: Tracks position, velocity, and acceleration from motor encoder feedback
• Safety Features: Detects stalled motors based on current and velocity measurements

2. PWMMotor (Motor Driver)

Manages the low-level motor control and measurement:

• Interfaces with the PWM rotatory encoder through abstract class PWMQdecInput
  • Abstract interface for encoder inputs:
    • Provides filtered encoder position values
    • Expects implementation with quadrature decoder hardware
    • Supports configurable gear ratios and sampling modes
• Controls motor direction and speed via PWM through abstract class PWMOutput
  • Abstract interface for PWM motor driver outputs:
    • Controls duty cycle (-1.0 to 1.0 range)
    • Supports bidirectional motor control
    • Two output channels for direction control
• Applies digital filtering to encoder measurements (2nd-order Biquad IIR filter)
  • Generic second-order Biquad IIR filter template:
    • Direct Form 1 implementation
    • Configurable coefficients for different filter characteristics
    • Automatic handling of non-finite values (LOCF: Last Observation Carried Forward)
    • Used to smooth encoder position measurements
• Handles motor polarity inversion for consistent control

Control Flow

update() [100 Hz cycle]
    ├── Kinematics Update
    │   ├── Sample motor position via PWMQdecInput
    │   ├── Apply Biquad filter
    │   ├── Calculate velocity (position delta / dt)
    │   └── Calculate acceleration (velocity delta / dt)
    │
    ├── Position Control
    │   ├── Calculate position error
    │   ├── Apply hysteresis (0.1 rad idle, 0.05 rad moving)
    │   └── Generate velocity setpoint (P-gain: 3.0)
    │
    └── Velocity Control
        ├── Calculate PID error
        │   ├── P-gain: 0.1
        │   ├── I-gain: 0.1
        │   └── D-gain: 0.001
        ├── Apply anti-windup saturation logic
        └── Generate motor duty cycle (-1.0 to 1.0)

Public API

Constructor

Servo(int motor_polarity, PWMQdecInput &motor_pwm_cnt, 
      PWMOutput &pwm_output_0, PWMOutput &pwm_output_1,
      getter_fn motor_current_getter, getter_fn supply_voltage_getter)

Parameters:

• motor_polarity: ±1 to control motor direction
• motor_pwm_cnt: Reference to encoder input interface
• pwm_output_0, pwm_output_1: PWM output channels for bidirectional control
• motor_current_getter: Function pointer to read motor current (A)
• supply_voltage_getter: Function pointer to read supply voltage (V)

Main Loop

void update()  // Call at ~100 Hz

Control Commands

• set_position(float position_setpoint): Set target position (radians)
• set_velocity(float velocity_setpoint): Set target velocity (rad/s)
• set_position_estimate(float position_estimate): Calibrate absolute position
• stop(): Immediately stop both motor outputs

State Queries

• get_servo_position_setpoint(): Return position command
• get_servo_velocity_setpoint(): Return velocity command
• get_servo_position_estimate(): Return filtered position (rad)
• get_servo_velocity_estimate(): Return calculated velocity (rad/s)
• get_servo_current_estimate(): Return motor current (A)
• get_servo_voltage_estimate(): Return motor voltage (V) = duty_cycle × supply_voltage
• is_servo_stalled(): Detect stall condition (velocity < 0.01 rad/s, |current| > 200 mA, |duty| > 50%)

Control Parameters

Position Control

Parameter	Value	Unit
Position P-gain	3.0	rad/s per rad
Hysteresis (idle)	0.1	rad
Hysteresis (moving)	0.05	rad
Max velocity output	8.0	rad/s
Crawl velocity	1.0	rad/s

Velocity Control (PID)

Parameter	Value	Unit
P-gain	0.1	-
I-gain	0.1	-
D-gain	0.001	-

Motor Driver

Parameter	Value
Resolution	100 steps
Duty cycle range	[-1.0, 1.0]
Supply voltage (nominal)	12 V

Position Filtering

The motor encoder position is filtered using a 2nd-order Biquad IIR filter with multiple preset options:

• LPF_O2_F20: Cutoff frequency at 20% of sampling rate (smoothest, ~20 Hz)
• LPF_O2_F04: Cutoff frequency at 4% of sampling rate (intermediate)
• LPF_O2_F002: Cutoff frequency at 0.2% of sampling rate (minimal filtering)

Usage Example

// Initialization with dependency injection
auto motor_current_getter = []() { return current_sensor.read(); };
auto supply_voltage_getter = []() { return battery_voltage.read(); };

Servo servo(1, motor_encoder, pwm_ch0, pwm_ch1, 
            motor_current_getter, supply_voltage_getter);

// Calibrate initial position
servo.set_position_estimate(0.0f);

// Main control loop
while (true) {
    // Set desired position
    servo.set_position(1.57f);  // π/2 radians

    // Run servo update (100 Hz)
    servo.update();

    // Monitor health
    if (servo.is_servo_stalled()) {
        // Handle stall condition
        servo.stop();
    }

    // Query state
    float position = servo.get_servo_position_estimate();
    float velocity = servo.get_servo_velocity_estimate();

    k_msleep(10);  // 100 Hz cycle
}