Position Feedback

RC servos don't provide position feedback, but BB.Servo.PCA9685.Sensor can estimate position based on commanded targets and timing. This tutorial shows you how to set up and use position feedback.

Prerequisites

Completed Basic Usage
A working servo joint with PCA9685 controller

Adding a Feedback Sensor

Add the sensor to your joint definition:

defmodule MyRobot do
  use BB.Robot

  robot do
    controller :pca9685, {BB.Servo.PCA9685.Controller,
      bus: "i2c-1",
      address: 0x40
    }

    link :base do
      joint :pan, type: :revolute do
        limit lower: ~u(-90 degree), upper: ~u(90 degree), velocity: ~u(60 degree_per_second)

        actuator :servo, {BB.Servo.PCA9685.Actuator, channel: 0, controller: :pca9685}
        sensor :feedback, {BB.Servo.PCA9685.Sensor, actuator: :servo}

        link :head do
        end
      end
    end
  end
end

The sensor requires the actuator option to know which actuator to subscribe to.

How Position Feedback Works

Since RC servos don't report their actual position, the sensor estimates it:

Actuator publishes commands - When you call set_position, the actuator publishes a PositionCommand message with the target angle and expected arrival time
Sensor subscribes - The sensor receives these messages and tracks the target position and expected arrival time
Position interpolation - During movement, the sensor interpolates between the previous position and target based on elapsed time
JointState publishing - The sensor publishes JointState messages with the estimated position

Timeline Example

Time 0ms:    Command sent (target: 45°, arrival: 500ms)
             Sensor receives command, starts interpolating

Time 100ms:  Estimated position: 9° (20% of the way)
Time 250ms:  Estimated position: 22.5° (50% of the way)
Time 500ms:  Estimated position: 45° (arrived)

Time 600ms:  Position stable at 45° (no change published)
Time 5000ms: Sync publish at 45° (max_silence reached)

Sensor Options

sensor :feedback, {BB.Servo.PCA9685.Sensor,
  actuator: :servo,           # Required: actuator to subscribe to
  publish_rate: ~u(50 hertz), # Optional: how often to check for changes (default: 50 Hz)
  max_silence: ~u(5 second)   # Optional: max time between publishes (default: 5s)
}

publish_rate

How often the sensor checks for position changes. Higher rates give smoother feedback during movement but use more resources.

~u(50 hertz) - Default, good for most applications
~u(100 hertz) - Smoother feedback for fast movements
~u(10 hertz) - Lower resource usage for slow-moving joints

max_silence

Even when the position hasn't changed, the sensor publishes periodically to keep subscribers in sync. This handles:

Late subscribers that missed earlier updates
Recovery from dropped messages
Monitoring systems that expect regular updates

Set to a higher value if you want less traffic when idle.

Subscribing to Position Updates

Subscribe to the sensor's JointState messages:

# Subscribe to the sensor topic
BB.subscribe(MyRobot, [:sensor, :pan, :feedback])

# In your GenServer or process
def handle_info(%BB.Message{payload: %BB.Message.Sensor.JointState{} = joint_state}, state) do
  [position] = joint_state.positions
  IO.puts("Pan position: #{position} radians")
  {:noreply, state}
end

Reading Current Position

You can also query the robot's state directly:

# Get current joint positions
state = BB.Robot.State.get(MyRobot)
pan_position = BB.Robot.State.get_joint_position(state, :pan)

Example: Position Logger

Here's a complete example that logs position changes:

defmodule PositionLogger do
  use GenServer

  def start_link(robot) do
    GenServer.start_link(__MODULE__, robot, name: __MODULE__)
  end

  def init(robot) do
    BB.subscribe(robot, [:sensor, :pan, :feedback])
    {:ok, %{robot: robot}}
  end

  def handle_info(%BB.Message{payload: %BB.Message.Sensor.JointState{} = js}, state) do
    [position] = js.positions
    degrees = position * 180 / :math.pi()
    IO.puts("[#{DateTime.utc_now()}] Pan: #{Float.round(degrees, 1)}°")
    {:noreply, state}
  end
end

Start the logger:

{:ok, _} = MyRobot.start_link()
{:ok, _} = PositionLogger.start_link(MyRobot)

# Move the servo and watch the logs
BB.Actuator.set_position(MyRobot, :servo, 0.785)
# Output:
# [2025-01-15 10:30:00.000000Z] Pan: 9.0°
# [2025-01-15 10:30:00.020000Z] Pan: 18.0°
# [2025-01-15 10:30:00.040000Z] Pan: 27.0°
# ... (interpolated positions during movement)
# [2025-01-15 10:30:00.500000Z] Pan: 45.0°

Example: Wait for Movement Complete

Wait for the servo to reach its target position:

defmodule ServoHelper do
  def move_and_wait(robot, actuator, target, timeout \\ 5000) do
    # Subscribe to sensor updates
    BB.subscribe(robot, [:sensor, :pan, :feedback])

    # Send the command
    BB.Actuator.set_position(robot, actuator, target)

    # Wait for position to match target
    wait_for_position(target, timeout)
  end

  defp wait_for_position(target, timeout) do
    receive do
      %BB.Message{payload: %BB.Message.Sensor.JointState{positions: [position]}}
      when abs(position - target) < 0.01 ->
        :ok
    after
      timeout -> {:error, :timeout}
    end
  end
end

# Usage
:ok = ServoHelper.move_and_wait(MyRobot, :servo, 0.785)
IO.puts("Servo reached target!")

Example: Multi-Joint Position Monitor

Monitor all servos in a hexapod leg:

defmodule LegMonitor do
  use GenServer

  @joints [:leg1_coxa, :leg1_femur, :leg1_tibia]

  def start_link(robot) do
    GenServer.start_link(__MODULE__, robot, name: __MODULE__)
  end

  def init(robot) do
    # Subscribe to all leg joint sensors
    for joint <- @joints do
      BB.subscribe(robot, [:sensor, joint, :feedback])
    end

    {:ok, %{robot: robot, positions: %{}}}
  end

  def handle_info(%BB.Message{frame_id: joint, payload: %BB.Message.Sensor.JointState{} = js}, state) do
    [position] = js.positions
    positions = Map.put(state.positions, joint, position)

    # Print all positions when we have updates for all joints
    if map_size(positions) == length(@joints) do
      print_leg_position(positions)
    end

    {:noreply, %{state | positions: positions}}
  end

  defp print_leg_position(positions) do
    coxa = rad_to_deg(positions[:leg1_coxa])
    femur = rad_to_deg(positions[:leg1_femur])
    tibia = rad_to_deg(positions[:leg1_tibia])

    IO.puts("Leg position: coxa=#{coxa}° femur=#{femur}° tibia=#{tibia}°")
  end

  defp rad_to_deg(rad), do: Float.round(rad * 180 / :math.pi(), 1)
end

Limitations

Remember that this is estimated position, not actual position:

The servo might not reach the target (blocked, insufficient torque)
The servo might overshoot or oscillate
The timing might not match the real servo exactly

For applications requiring precise position feedback, consider:

Adding a physical sensor (potentiometer, encoder) to your servo
Using a servo with built-in feedback (smart servos)
Using stepper motors with encoders

Comparison with BB.Servo.Pigpio

The sensor works identically to BB.Servo.Pigpio.Sensor. If you're migrating from pigpio to PCA9685, simply:

Replace BB.Servo.Pigpio.Sensor with BB.Servo.PCA9685.Sensor
Ensure the actuator option points to a PCA9685 actuator

The message types and behaviour are the same.

Next Steps

You now have a complete servo setup with position feedback. Explore the BB framework documentation to learn about:

Trajectory planning for smooth multi-joint movements
Inverse kinematics for end-effector positioning
State machines for complex behaviours

← Previous Page Basic Usage