Control package

Encoded motors and sensor-controlled bases use a control loop implemented by viam-server. You can configure the control_parameters attribute for both component types to adjust the control loop.

If you need to change or customize the control loops beyond the configurable parameters, or you want to add a control loop to a different component, you can use the control package from the RDK to build your own PID control loop.

The control package implements feedback control on an endpoint, which is usually the hardware you are trying to control. With the control package, you can design a control loop that monitors a process variable (PV) and compares it with a set point (SP). The control loop generates a control action to reduce the error value between the SP and PV ( SP - PV) to zero.

Control loops are represented in a diagrammatic style known as a block diagram. Each block represents a transfer function of a component. The control loop is broken down into successive “blocks” connected by their dependencies.

Creating and using a PID control loop

A PID control loop computes a correction for the error value between SP and PV using three terms:

• A proportional term that is the current error
• An integral term that is the total cumulative error
• A derivative term that is the rate of change of the error

By tuning the coefficients on each of these terms, you can adjust how your system converges towards the target value, how quickly it reaches the target, and how much it overshoots.

The following functions are available for creating and using a control loop:

SetupPIDControlConfig

Creates a PIDLoop object, which contains all the attributes related to a control loop that a controlled component needs, including the control config. If all PID values are zero and NeedsAutoTuning is set, the loop runs auto-tuning on creation.

Parameters:

• pidVals ([]PIDConfig): The P, I, and D values for the control loop. If all are zero and NeedsAutoTuning is true, the loop auto-tunes and logs the calculated values.
• componentName (string): The name of the component that the PID loop controls.
• options (Options): Optional parameters to customize the control loop.
• c (Controllable): An interface that contains the functions to move the controlled component.
• logger (logging.Logger): The logger of the controlled component.

Returns:

• (*PIDLoop): A pointer to a struct containing all relevant control loop attributes.
• (error): An error, if one occurred.

pidVals := []control.PIDConfig{{
  Type: "",
  P: 1.0,
  I: 2.0,
  D: 0.0,
}}

options := control.Options{
  PositionControlUsingTrapz: true,
  LoopFrequency:             100.0,
}

pidLoop, err := control.SetupPIDControlConfig(pidVals, "motor_name", options, motor, motor.logger)

TunePIDLoop

Runs auto-tuning on the control loop to determine optimal PID values. Tuning runs in a background goroutine. When tuning completes, the component logs the calculated PID values and returns a TunedPIDErr prompting you to copy the values into your config.

Parameters:

• ctx (Context): A context for the tuning operation.
• cancelFunc (CancelFunc): A cancel function to stop tuning.

Returns:

• (error): An error, if one occurred.

cancelCtx, cancelFunc := context.WithCancel(context.Background())
err := pidLoop.TunePIDLoop(cancelCtx, cancelFunc)

StartControlLoop

Starts running the PID control loop in a background thread to monitor and adjust the inputs to the controlled component.

Parameters:

• None

Returns:

• (error): An error, if one occurred.

err := pidLoop.StartControlLoop()

CreateConstantBlock

Creates a new control block of type constant.

Parameters:

• ctx (Context): A context for the operation.
• name (string): The name of the constant block.
• constVal (float64): The value of the new set point.

Returns:

• (BlockConfig): The config for the newly created block.

constBlock := control.CreateConstantBlock(context.Background(), "set_point", 10.0)

UpdateConstantBlock

Creates a new constant block and updates the running control loop to use it.

Parameters:

• ctx (Context): A context for the operation.
• name (string): The name of the constant block.
• constVal (float64): The value of the new set point.
• loop (*Loop): The running control loop to update.

Returns:

• (error): An error, if one occurred.

err := control.UpdateConstantBlock(context.Background(), "set_point", 10.0, loop)

CreateTrapzBlock

Creates a new control block of type trapezoidalVelocityProfile.

Parameters:

• ctx (Context): A context for the operation.
• name (string): The name of the trapezoidal block.
• maxVel (float64): The max velocity for the controlled component.
• dependsOn ([]string): The names of the other control blocks that this block depends on. Usually the set point and the endpoint.

Returns:

• (BlockConfig): The config for the newly created block.

trapzBlock := control.CreateTrapzBlock(
  context.Background(), "trapz", 4000.0, []string{"set_point", "endpoint"},
)

UpdateTrapzBlock

Creates a new trapezoidal velocity profile block and updates the running control loop to use it.

Parameters:

• ctx (Context): A context for the operation.
• name (string): The name of the trapezoidal block.
• maxVel (float64): The max velocity for the controlled component. Must be non-zero.
• dependsOn ([]string): The names of the other control blocks that this block depends on.
• loop (*Loop): The running control loop to update.

Returns:

• (error): An error, if one occurred.

err := control.UpdateTrapzBlock(
  context.Background(), "trapz", 4000.0, []string{"set_point", "endpoint"}, loop,
)

Data types

PIDLoop

PIDLoop contains all the attributes for setting up a PID control loop. SetupPIDControlConfig creates this struct for you.

type PIDLoop struct {
  BlockNames   map[string][]string
  PIDVals      []PIDConfig
  TunedVals    *[]PIDConfig
  ControlConf  *Config
  ControlLoop  *Loop
  Options      Options
  Controllable Controllable
}

PIDConfig

PIDConfig holds the PID values for a control loop. For 2-dimensional control loops (such as a sensor-controlled base which controls both linear and angular velocity), the Type field is required and must be either "linear_velocity" or "angular_velocity". For 1-dimensional control loops (such as an encoded motor), the Type field is not necessary.

type PIDConfig struct {
  Type string  `json:"type,omitempty"`
  P    float64 `json:"p"`
  I    float64 `json:"i"`
  D    float64 `json:"d"`
}

Controllable

Controllable is an interface that any component must implement to be controlled by a control loop. Encoded motors and sensor-controlled bases already implement this interface.

• State() gets the current state of the endpoint (controlled component) and passes that information to the next iteration of the control loop.
• SetState() takes the output from the latest iteration of the control loop and sets the state of the endpoint.

For example, in an encoded motor, State() measures the current position of the motor so the next iteration knows how far it is from the goal. The control loop calculates the next power percentage, and SetState() applies that power to the motor.

type Controllable interface {
  SetState(ctx context.Context, state []*Signal) error
  State(ctx context.Context) ([]float64, error)
}

Options

Options contains optional parameters for customizing a control loop. All fields have defaults, so you only need to set the ones you want to change.

type Options struct {
  PositionControlUsingTrapz           bool
  SensorFeedback2DVelocityControl     bool
  DerivativeType                      string
  UseCustomConfig                     bool
  CompleteCustomConfig                Config
  NeedsAutoTuning                     bool
  LoopFrequency                       float64
  ControllableType                    string
}

The built-in control loop setup works with encoded motors and sensor-controlled bases. To use a different setup, set UseCustomConfig to true and CompleteCustomConfig to your custom Config. See Control blocks for how to build individual blocks.

BlockConfig

BlockConfig is the configuration for an individual control block. You need to build BlockConfig structs when using the UseCustomConfig option. Each block type requires different attributes, documented in Control blocks.

type BlockConfig struct {
  Name      string             `json:"name"`
  Type      controlBlockType   `json:"type"`
  Attribute utils.AttributeMap `json:"attributes"`
  DependsOn []string           `json:"depends_on"`
}

Config

Config is the top-level control loop configuration containing a list of blocks and a loop frequency.

type Config struct {
  Blocks    []BlockConfig `json:"blocks"`
  Frequency float64       `json:"frequency"`
}

Control blocks

The following example is a block diagram of a control loop that controls the speed of a motor. The motor has an encoder that reports its position. The position is derived to get speed, then filtered to remove noise. The error ( SP - PV) is fed into the PID controller, which outputs a correction through a gain block to the endpoint.

+-------------+       +----------+       +----------+       +----------+
|  SetPoint   +------>|   Sum    +------>|   PID    +------>| Endpoint |
| (constant)  |       |          |       |          |       |  (motor) |
+-------------+       +-----^----+       +----------+       +----+-----+
                             |                                    |
                       +-----+----+       +----------+            |
                       |  Filter  |<------+ Derive   |<-----------+
                       |          |       |          |
                       +----------+       +----------+

The frequency field in the control config sets how often the loop runs (in Hz). Higher frequencies produce smaller errors at each step and smoother control.

Common block fields

All blocks share these fields:

• name: A unique name used to reference the block in depends_on lists.
• type: The block type (see supported types below).
• attributes: Type-specific configuration.
• depends_on: List of block names this block takes input from.

Gain

Multiplies the input signal by a constant gain value: S_out = S_in * gain.

{
  "name": "gain",
  "type": "gain",
  "attributes": {
    "gain": 0.00392156862
  },
  "depends_on": ["PID"]
}

The default gain of 0.00392156862 (1/255) scales the PID output to a 0-1 PWM range for standard 8-bit PWM boards.

Constant

Outputs a constant signal value, typically used as the set point: S_out = constant_val.

{
  "name": "set_point",
  "type": "constant",
  "attributes": {
    "constant_val": 0.0
  }
}

Endpoint

Represents the physical hardware being controlled. The endpoint reads state from the hardware and applies control outputs to it.

Set the attribute key to "motor_name" for motors or "base_name" for bases.

{
  "name": "endpoint",
  "type": "endpoint",
  "attributes": {
    "motor_name": "my-motor"
  },
  "depends_on": ["gain"]
}

PID

The PID block computes a correction from the error signal using proportional, integral, and derivative terms:

u(t) = Kp * e(t) + Ki * integral(e(t)) * dt + Kd * (de(t)/dt)

Where Kp, Ki, and Kd are the PID gains, e(t) is the error at time t, and dt is the time between steps.

The PIDSets attribute takes an array of PIDConfig objects. Use one PID set for single-axis control (encoded motor) or two for dual-axis control (sensor-controlled base with linear and angular velocity).

{
  "name": "PID",
  "type": "PID",
  "attributes": {
    "PIDSets": [{ "p": 1.0, "i": 0.5, "d": 0.0 }],
    "limit_up": 255.0,
    "limit_lo": -255.0,
    "int_sat_lim_up": 255.0,
    "int_sat_lim_lo": -255.0,
    "tune_ssr_value": 2.0,
    "tune_method": "ziegerNicholsPI",
    "tune_step_pct": 0.35
  },
  "depends_on": ["sum"]
}

Tuning methods

To auto-tune, set all PID values to 0. The control loop records the step response and uses the relay method to estimate the ultimate gain (Ku) and oscillation period (Tu), then calculates gains using the selected method:

Encoder to RPM

Converts encoder counts to RPM using the pulses-per-revolution value of the encoder.

{
  "name": "encoder_to_rpm",
  "type": "encoderToRpm",
  "attributes": {
    "PulsesPerRevolution": 14
  },
  "depends_on": ["endpoint"]
}

Sum

Sums input signals according to a sum_string that specifies the sign of each input. Each character is + or -, one per dependency in order.

{
  "name": "sum",
  "type": "sum",
  "attributes": {
    "sum_string": "+-"
  },
  "depends_on": ["set_point", "endpoint"]
}

In this example, the output is set_point - endpoint, computing the error between the desired and actual values.

Trapezoidal velocity profile

Position control of a motor can use a trapezoidal velocity profile generator. When a new set point arrives, this block generates a velocity profile with three phases: acceleration, constant speed, and deceleration.

The profile is dynamically adjusted during deceleration to keep the end position within the position window. The block also acts as a deadband controller when the target position is reached, preventing the motor from moving outside the window.

{
  "name": "trapz",
  "type": "trapezoidalVelocityProfile",
  "attributes": {
    "max_vel": 4000.0,
    "max_acc": 30000.0,
    "pos_window": 10.0,
    "kpp_gain": 0.45
  },
  "depends_on": ["set_point", "endpoint"]
}

Derivative

Computes the derivative of the input signal using a finite difference method.

{
  "name": "derivative",
  "type": "derivative",
  "attributes": {
    "derive_type": "backward1st1"
  },
  "depends_on": ["endpoint"]
}

Filter

Applies a digital filter to the input signal to remove noise. Supports FIR (finite impulse response) and IIR (infinite impulse response) filters.

{
  "name": "filter",
  "type": "filter",
  "attributes": {
    "filter_type": "lowpass"
  },
  "depends_on": ["derivative"]
}

Examples

Using SetupPIDControlConfig

Set up and tune a control loop using SetupPIDControlConfig:

// Set the options for your component
options := control.Options{
  LoopFrequency: 100.0,
}

// Create PID values. All zeros triggers auto-tuning.
controlParams := []control.PIDConfig{{
  Type: "",
  P:    0.0,
  I:    0.0,
  D:    0.0,
}}

// Enable auto-tuning if all PID values are 0
if controlParams[0].NeedsAutoTuning() {
  options.NeedsAutoTuning = true
}

// Create the control config and tune if necessary
pidLoop, err := control.SetupPIDControlConfig(
  controlParams, "component", options, component, component.logger,
)
if err != nil {
  return err
}

// Useful attributes after setup
controlLoopConfig := pidLoop.ControlConf
loop := pidLoop.ControlLoop
blockNames := pidLoop.BlockNames

Using a custom control config

Set up a control loop with a custom block configuration using the UseCustomConfig and CompleteCustomConfig options:

// Create a custom control config
controlConfig := control.Config{
  Blocks: []control.BlockConfig{
    {
      Name: "set_point",
      Type: "constant",
      Attribute: rdkutils.AttributeMap{
        "constant_val": 0.0,
      },
    },
    {
      Name: "sum",
      Type: "sum",
      Attribute: rdkutils.AttributeMap{
        "sum_string": "+-",
      },
      DependsOn: []string{"set_point", "endpoint"},
    },
    {
      Name: "PID",
      Type: "PID",
      Attribute: rdkutils.AttributeMap{
        "int_sat_lim_lo": -255.0,
        "int_sat_lim_up": 255.0,
        "PIDSets":        []*control.PIDConfig{{P: 1.0, I: 0.5, D: 0.0}},
        "limit_lo":       -255.0,
        "limit_up":       255.0,
        "tune_method":    "ziegerNicholsPI",
        "tune_ssr_value": 2.0,
        "tune_step_pct":  0.35,
      },
      DependsOn: []string{"sum"},
    },
    {
      Name: "gain",
      Type: "gain",
      Attribute: rdkutils.AttributeMap{
        "gain": 0.00392156862,
      },
      DependsOn: []string{"PID"},
    },
    {
      Name: "endpoint",
      Type: "endpoint",
      Attribute: rdkutils.AttributeMap{
        "motor_name": "my-motor",
      },
      DependsOn: []string{"gain"},
    },
  },
  Frequency: 100.0,
}

// Set the options to use the custom config
options := control.Options{
  UseCustomConfig:      true,
  CompleteCustomConfig: controlConfig,
}

// Create PID parameters for auto-tuning
controlParams := []control.PIDConfig{{
  Type: "",
  P:    0.0,
  I:    0.0,
  D:    0.0,
}}

if controlParams[0].NeedsAutoTuning() {
  options.NeedsAutoTuning = true
}

// Create and configure the PID loop
pidLoop, err := control.SetupPIDControlConfig(
  controlParams, "my-motor", options, component, component.logger,
)
if err != nil {
  return err
}

// Useful attributes after setup
controlLoopConfig := pidLoop.ControlConf
loop := pidLoop.ControlLoop
blockNames := pidLoop.BlockNames