Creating a Slithering Snake Robot with Obstacle Avoidance Using Pybricks

Building a snake-like robot that navigates autonomously while dodging obstacles is a fascinating robotics challenge. Below, I’ll break down how this Pybricks code (for LEGO MINDSTORMS Robot Inventor) brings a serpentine bot to life. The robot uses sine-wave motion for propulsion and ultrasonic sensing for obstacle detection. I originally programmed the snake in Robot Inventor Python, but Pybricks makes the code so much easier to read, so I ported and rebuilt the code.

How the Snake Robot Works

1. Undulating Motion
   Motors in each segment follow a sine-wave pattern, creating a traveling wave that propels the robot forward. The wave propagates from tail to head when moving straight.
2. Direction Control
   Pressing the hub’s left/right buttons adjusts the phase offset between segments, steering the robot by shifting the wave direction.
3. Obstacle Avoidance
   An ultrasonic sensor on the “head” detects obstacles within 15 cm. When triggered, the robot:
   • Temporarily bends its entire body sideways (TURN = 30°).
   • Resumes straight motion after one undulation cycle (PERIOD = 3500 ms).
   • Head LEDs change color to indicate avoidance mode.

Annotated Code

Let’s start with the code first. I have added comments to make it easy to follow. Then, read on to find more explanation of the movement principles and wave terminology like ‘phase’, ‘offset’, and ‘period’.

from pybricks.pupdevices import Motor, UltrasonicSensor
from pybricks.hubs import InventorHub
from pybricks.parameters import Port, Button
from umath import sin, pi  # Import sine and pi for wave math
from pybricks.tools import StopWatch, wait

# --- Constants ---
PERIOD = 3500             # Wave cycle duration (ms)
TIME_FACTOR = 2*pi / PERIOD  # Convert time to radians
MAX_ANGLE = 40            # Max motor swing (degrees)
OFFSET_INCREMENT = -pi/15 # Phase shift per button press (radians)
TURN = 30                 # Avoidance turn angle (degrees)

# --- Hardware Setup ---
m1 = Motor(Port.C)        # Tail motor
m2 = Motor(Port.D)        # Mid-motor 1
m3 = Motor(Port.E)        # Mid-motor 2
m4 = Motor(Port.F)        # Head motor
head = UltrasonicSensor(Port.A)  # Obstacle detector
hub = InventorHub()       # Central hub
motors = [m1, m2, m3, m4]  # All motors in order
sw = StopWatch()          # Tracks wave timing
turn_sw = StopWatch()     # Times avoidance maneuvers

# --- Initial State ---
offset = 0                # Current phase shift
offset_number = 0         # Button-press counter
turn = 0                  # Avoidance turn (0 when straight)
head.lights.on([0,0,100,100])  # Blue headlights (idle)

# --- Main Loop ---
while True:
    # 1. Update Motor Positions (Snake Wave)
    i = 0
    for m in motors:
        # Calculate motor angle: 
        #   Sine wave + segment offset + avoidance turn
        angle = MAX_ANGLE * sin(sw.time() * TIME_FACTOR + i * offset) + turn
        m.track_target(angle)  # Move motor to target
        i += 1

    # 2. Obstacle Avoidance Logic
    if head.distance() < 150 and abs(offset_number) > 1:
        turn_sw.reset()              # Start turn timer
        turn = TURN                  # Bend body sideways
        head.lights.on([100,100,100,100])  # White headlights (avoiding)

    # Return straight after one wave cycle
    if turn == TURN and turn_sw.time() > PERIOD:
        turn = 0                     # Reset turn
        head.lights.on([0,0,50,50])  # Dim blue (resuming)

    # 3. Steering via Hub Buttons
    btns = hub.buttons.pressed()
    if btns:
        if Button.LEFT in btns:
            offset_number -= 1       # Shift wave left
        elif Button.RIGHT in btns:
            offset_number += 1       # Shift wave right

        offset = OFFSET_INCREMENT * offset_number  # Apply phase shift
        wait(300)                    # Debounce delay
        hub.display.number(offset_number)  # Show steering intensity

Understanding Wave Motion in Snake Robots: Phase, Offset, and Period

In the context of the snake robot’s movement, these terms describe how the sine wave controlling the motors creates traveling waves along the body. Here’s a breakdown.

1. Period (PERIOD = 3500 ms)

• What it is: The time for one complete wave cycle.
• Snake analogy: How long it takes for one “S-shaped” undulation to travel from head to tail.
• In practice:
• PERIOD = 3500 ms means the wave repeats every 3.5 seconds.
• Controls the robot’s speed:
  python TIME_FACTOR = 2*pi / PERIOD # Converts time to wave position

2. Phase

• What it is: The current position in the wave cycle (0 to 2π radians).
• Snake analogy: Where in the “S-curve” a body segment is at any moment.
• In the code:

  angle = MAX_ANGLE * sin(sw.time() * TIME_FACTOR + ...)

• sw.time() * TIME_FACTOR calculates the phase based on elapsed time.
• At time t=0, phase=0 → middle of the wave (motor angle=0°).
• At phase=π/2 → peak of the wave (motor angle=+40°).

3. Offset (offset = -π/15 * offset_number)

• What it is: A deliberate phase shift between adjacent motors.
• Snake analogy: How much each segment lags behind the previous one.
• In the code:

  angle = ... + i * offset  # i = motor index (0,1,2,3)

• For motor i=0 (tail): phase = base_phase + 0*offset
• For motor i=1: phase = base_phase + 1*offset
• For motor i=2: phase = base_phase + 2*offset

Why it matters

• offset = 0 → All motors move together (no wave, robot doesn’t advance).
• offset = -π/15 (≈ -12°) → Each motor lags 12° behind the previous one, creating a traveling wave from tail to head.

Visualizing the Wave

This staggered motion (offset) creates a propagating wave that pushes against the ground, moving the robot forward.

Steering via Offset

• Left button press:

  offset_number -= 1 → offset = -π/15 * (negative number)

• Negative offset → Wave travels left-to-right → Robot turns right.
• Right button press:

  offset_number += 1 → offset = -π/15 * (positive number)

• Positive offset → Wave travels right-to-left → Robot turns left.

Key Insight

The combination of period (wave speed), phase (instantaneous motor position), and offset (inter-motor delay) creates biomimetic serpentine motion. Adjusting these values lets you tune the robot’s speed, gait, and turning radius!

In short: Key Snake Movement Mechanisms Explained

• Sine Wave Propulsion:
  Each motor’s angle follows MAX_ANGLE * sin(time + phase_offset). The offset creates a delay between adjacent motors, forming a traveling wave.
  Example: offset = -π/15 shifts each motor 12° behind the previous one.
• Steering:
  Pressing LEFT/RIGHT increments offset_number, increasing the phase shift. This skews the wave direction, turning the robot:
• Positive offset_number → Right turn
• Negative offset_number → Left turn
• Obstacle Response:
  When the ultrasonic sensor detects an obstacle (<150 mm), the robot overrides all motors with a fixed TURN angle. After one wave cycle (PERIOD), it reverts to sine motion.

Tips for Customization

1. Adjust Responsiveness:
   Modify OFFSET_INCREMENT to change steering sensitivity (larger values = sharper turns).
2. Tune Motion:
   Increase MAX_ANGLE for wider slithers or reduce PERIOD for faster movement.
3. Enhance Avoidance:
   Change TURN or add backward motion by setting negative angles during avoidance.

This implementation elegantly combines trigonometry for biomimetic motion with reactive obstacle avoidance—all in under 50 lines! The result is a mesmerizing robotic snake that navigates spaces with lifelike grace.