🤖 Level 4 – Mobile Robotics
Project 4.3: "Ultrasonic Sensor with Servo"
What you’ll learn
- Goal 1: Read static and live distances using the official ultrasonic block
sonar.Sonar(TRIG,ECHO).checkdist(). - Goal 2: Sweep an SG90 servo with the official block
servo.servo180_angle(pin, angle)to scan the environment. - Goal 3: Build a 180‑degree map by sampling distances at multiple angles.
- Goal 4: Detect obstacles in real time and trigger simple alerts.
- Goal 5: Integrate scanning with movement decisions (forward, turn, stop) in a friendly loop.
Key ideas
- Use the official blocks: sonar for distance, servo for angle — no custom timing needed.
- Safety: Clamp angles, add small delays, handle None/timeouts gracefully.
- Clarity: Short logs, simple helpers, steady pacing to avoid frustration.
Blocks glossary
- sonar.Sonar(TRIG, ECHO): Creates an ultrasonic sensor object with TRIG and ECHO pins.
- .checkdist(): Returns distance (cm). If no echo, may return an invalid or None‑like value.
- servo.servo180_angle(pin, angle): Sets an SG90 servo to a specific angle (0–180).
- print(): Sends serial logs to the PC for clarity and debugging.
- def function: Wraps actions (set_angle, read_cm, sweep) to keep code easy to read.
- Loop: Repeats scanning and decisions at a comfortable rhythm.
What you need
| Part | How many? | Pin connection (R32) |
|---|---|---|
| D1 R32 | 1 | USB cable |
| HC‑SR04 ultrasonic | 1 | TRIG → Pin 26, ECHO → Pin 5 |
| SG90 servo | 1 | Signal → Pin 23, VCC 5V (external), GND (shared) |
| External 5V supply | 1 | Servo and sensor power (share GND with R32) |
- Share GND between R32, ultrasonic, and servo.
- Avoid powering the servo from the USB 3.3V — use a proper 5V supply. ***revisar***
Before you start
- Wire the ultrasonic (TRIG 26, ECHO 5) and servo (Signal 23) carefully.
- Open the serial monitor.
- Quick test:
print("Ready!") # Confirm serial is working
Microprojects 1–5
Microproject 4.3.1 – Static distance measurement (official sonar block)
# Microproject 4.3.1 – Static distance measurement using sonar.Sonar block
import machine # Load hardware library for pins
import sonar # Load official ultrasonic helper
us = sonar.Sonar(26, 5) # Create ultrasonic object (TRIG=26, ECHO=5)
print("[US] Init TRIG=26 ECHO=5") # Serial: confirm ultrasonic setup
d = us.checkdist() # Read distance in centimeters (cm)
print("[US] Distance (cm):", d) # Serial: show the distance result
# Note: If no echo returned, d may be None or an invalid value depending on the driver.
# Always check and handle that case in later microprojects.
Reflection: You measured distance with the official block — clean and simple.
Challenge: Take 3 readings and print the minimum and average to reduce noise.
Microproject 4.3.2 – Servo sweep for scanning (official servo block)
# Microproject 4.3.2 – Servo sweep using servo.servo180_angle
import servo # Load official servo helper
import time # Load time library for delays
PIN_SERVO = 23 # Define servo signal pin
print("[Servo] Signal pin =", PIN_SERVO) # Serial: confirm servo pin
def set_angle(a): # Helper: set servo angle safely
if a < 0: # If angle less than 0
a = 0 # Clamp to 0
if a > 180: # If angle greater than 180
a = 180 # Clamp to 180
servo.servo180_angle(PIN_SERVO, a) # Use official block to set angle
print("[Servo] Angle set to", a) # Serial: log angle
time.sleep_ms(250) # Small settle delay for smooth movement
for a in range(0, 181, 30): # Sweep forward in 30° steps
set_angle(a) # Set target angle
for a in range(180, -1, 30): # Sweep back in 30° steps
set_angle(a) # Set target angle
Reflection: You used the servo block to sweep angles with safe limits and clear pacing.
Challenge: Use 15° steps near edges (0–30 and 150–180) for smoother ease‑in/out.
Microproject 4.3.3 – 180‑degree environment mapping (samples list)
# Microproject 4.3.3 – Build a simple 180° map using servo + sonar
import servo # Load official servo helper
import sonar # Load official ultrasonic helper
import time # Load time library
PIN_SERVO = 23 # Servo signal pin
us = sonar.Sonar(26, 5) # Ultrasonic: TRIG=26, ECHO=5
print("[Map] Servo=23, TRIG=26, ECHO=5") # Serial: confirm pins
def set_angle(a): # Helper: clamp and set angle
if a < 0: # Clamp low
a = 0 # To 0
if a > 180: # Clamp high
a = 180 # To 180
servo.servo180_angle(PIN_SERVO, a) # Official block sets angle
print("[Map] Angle:", a) # Serial: log angle
time.sleep_ms(250) # Small settle delay
def read_cm(): # Helper: read ultrasonic cm
d = us.checkdist() # Official block returns distance
print("[Map] Dist cm:", d) # Serial: log raw distance
return d # Return distance for storage
samples = [] # Create list to store (angle, distance)
for a in range(0, 181, 15): # Sweep 0° to 180° every 15°
set_angle(a) # Move servo to angle
d = read_cm() # Read distance at angle
samples.append((a, d)) # Store tuple (angle, distance)
print("[Map] Sample", a, "=", d) # Serial: show sample
print("[Map] Total samples:", len(samples)) # Serial: confirm sample count
Reflection: You created a clear list of angle‑distance pairs — a tiny world map.
Challenge: Replace None/invalid readings with the last valid value to smooth the map.
Microproject 4.3.4 – Real‑time obstacle detection (simple alert)
# Microproject 4.3.4 – Continuous detection + alert using official sonar
import machine # Load hardware library
import sonar # Load ultrasonic helper
import time # Load time library
led = machine.Pin(13, machine.Pin.OUT) # On-board LED for alerts
us = sonar.Sonar(26, 5) # Ultrasonic TRIG=26, ECHO=5
print("[Detect] LED=13, TRIG=26, ECHO=5") # Serial: confirm hardware
ALERT_CM = 20 # Alert distance threshold (cm)
def is_valid(d): # Helper: validate reading
return (d is not None) and (d > 0) # True if a positive number
while True: # Continuous detection loop
d = us.checkdist() # Read distance in cm
if not is_valid(d): # If invalid or None reading
led.value(0) # LED OFF (no alert)
print("[Detect] Timeout/Invalid") # Serial: log invalid status
elif d <= ALERT_CM: # If object at/below threshold
led.value(1) # LED ON (alert)
print("[Detect] CLOSE:", d, "cm") # Serial: log close object
else: # Otherwise safe distance
led.value(0) # LED OFF
print("[Detect] OK:", d, "cm") # Serial: log normal distance
time.sleep_ms(150) # Short delay for responsiveness
Reflection: You built a friendly detector that stays calm and informative.
Challenge: Blink the LED for 1 second when a close object is first detected (cooldown).
Microproject 4.3.5 – Integration with robot movement (decide and act)
# Microproject 4.3.5 – Scan center/left/right and choose movement actions
import machine # Load hardware library for motor pins
import sonar # Load official ultrasonic helper
import servo # Load official servo helper
import time # Load time library
# Ultrasonic setup
us = sonar.Sonar(26, 5) # Ultrasonic TRIG=26, ECHO=5
print("[Integrate] Ultrasonic ready") # Serial: confirm ultrasonic setup
# Servo setup
PIN_SERVO = 23 # Servo signal pin
print("[Integrate] Servo pin =", PIN_SERVO) # Serial: confirm servo pin
# Motor driver pins (example; adjust to your L298N wiring)
in1 = machine.Pin(21, machine.Pin.OUT) # Motor IN1 (left)
in2 = machine.Pin(13, machine.Pin.OUT) # Motor IN2 (left)
in3 = machine.Pin(27, machine.Pin.OUT) # Motor IN3 (right)
in4 = machine.Pin(26, machine.Pin.OUT) # Motor IN4 (right)
print("[Integrate] Motors IN1=21 IN2=13 IN3=27 IN4=26") # Serial: confirm pins
SAFE_CM = 25 # Safe distance threshold (cm)
def set_angle(a): # Helper: clamp and set angle
if a < 0: # If under 0
a = 0 # Clamp to 0
if a > 180: # If over 180
a = 180 # Clamp to 180
servo.servo180_angle(PIN_SERVO, a) # Official servo angle block
print("[Scan] Angle:", a) # Serial: show angle
time.sleep_ms(220) # Small settle delay
def read_cm(): # Helper: read distance
d = us.checkdist() # Official ultrasonic distance in cm
print("[Scan] Dist:", d) # Serial: show distance
return d # Return cm for decisions
def stop(): # Helper: stop both tracks
in1.value(0); in2.value(0) # Left motor OFF
in3.value(0); in4.value(0) # Right motor OFF
print("[Move] STOP") # Serial: movement log
def forward(): # Helper: move forward
in1.value(1); in2.value(0) # Left forward
in3.value(1); in4.value(0) # Right forward
print("[Move] FORWARD") # Serial: movement log
def left(): # Helper: turn left
in1.value(0); in2.value(1) # Left backward
in3.value(1); in4.value(0) # Right forward
print("[Move] LEFT") # Serial: movement log
def right(): # Helper: turn right
in1.value(1); in2.value(0) # Left forward
in3.value(0); in4.value(1) # Right backward
print("[Move] RIGHT") # Serial: movement log
while True: # Main scan-decide-act loop
set_angle(90) # Look straight ahead (center)
d_center = read_cm() # Read center distance
if (d_center is None) or (d_center <= 0): # If invalid center reading
stop() # Stop to be safe
elif d_center < SAFE_CM: # If obstacle close ahead
set_angle(30) # Look left
d_left = read_cm() # Read left distance
set_angle(150) # Look right
d_right = read_cm() # Read right distance
if (d_left or 0) > (d_right or 0): # If left clearer
left() # Turn left
else: # Else right clearer or equal
right() # Turn right
time.sleep_ms(600) # Move briefly
stop() # Stop and reassess
else: # Path ahead is clear
forward() # Move forward
time.sleep_ms(600) # Move a bit
stop() # Stop and re-scan
time.sleep_ms(250) # Loop pacing for calm behavior
Reflection: You made your robot “look” left and right to navigate with simple, kind logic.
Challenge: Add PWM speed (slow when close, faster when far) to make motion feel intelligent.
Main project
Ultrasonic + servo scanning with obstacle avoidance and movement decisions (official blocks)
- Ultrasonic:
sonar.Sonar(TRIG,ECHO).checkdist()reads clean distance values. - Servo:
servo.servo180_angle(pin, angle)positions the sensor at 0–180°. - Mapping: Sample multiple angles and store
(angle, distance)pairs. - Detection: LED alerts and safe decisions when obstacles are close.
- Movement: Forward/left/right/stop choices based on scan results.
# Project 4.3 – Official Blocks Integration (sonar + servo + movement)
import machine # Load hardware pin library
import sonar # Load official ultrasonic helper
import servo # Load official servo helper
import time # Load time library
# Ultrasonic sensor
us = sonar.Sonar(26, 5) # TRIG=26, ECHO=5 per reference
print("[Main] Ultrasonic TRIG=26 ECHO=5") # Serial: confirm ultrasonic
# Servo setup
PIN_SERVO = 23 # Servo signal pin per reference
print("[Main] Servo pin =", PIN_SERVO) # Serial: confirm servo
# Motor pins (example wiring for L298N)
in1 = machine.Pin(21, machine.Pin.OUT) # Motor IN1
in2 = machine.Pin(13, machine.Pin.OUT) # Motor IN2
in3 = machine.Pin(27, machine.Pin.OUT) # Motor IN3
in4 = machine.Pin(26, machine.Pin.OUT) # Motor IN4
print("[Main] Motors set") # Serial: confirm motors
SAFE_CM = 25 # Safe distance threshold in cm
def set_angle(a): # Helper: clamp and set servo angle
if a < 0: # If below 0
a = 0 # Clamp to 0
if a > 180: # If above 180
a = 180 # Clamp to 180
servo.servo180_angle(PIN_SERVO, a) # Official block sets angle
print("[Main] Angle", a) # Serial: show angle
time.sleep_ms(220) # Small settle delay
def read_cm(): # Helper: read ultrasonic distance cm
d = us.checkdist() # Official block returns cm
print("[Main] Dist", d) # Serial: show value
return d # Return for decisions
def stop(): # Helper: stop movement
in1.value(0); in2.value(0) # Left OFF
in3.value(0); in4.value(0) # Right OFF
print("[Main] STOP") # Serial: movement log
def forward(): # Helper: forward
in1.value(1); in2.value(0) # Left forward
in3.value(1); in4.value(0) # Right forward
print("[Main] FORWARD") # Serial: movement log
def left(): # Helper: turn left
in1.value(0); in2.value(1) # Left backward
in3.value(1); in4.value(0) # Right forward
print("[Main] LEFT") # Serial: movement log
def right(): # Helper: turn right
in1.value(1); in2.value(0) # Left forward
in3.value(0); in4.value(1) # Right backward
print("[Main] RIGHT") # Serial: movement log
# Welcome
print("[Main] Scan + Avoid starting") # Serial: start
while True: # Main loop
set_angle(90) # Look center
d_center = read_cm() # Read center distance
if (d_center is None) or (d_center <= 0): # If invalid
stop() # Be safe
elif d_center < SAFE_CM: # If close obstacle
set_angle(30) # Look left
d_left = read_cm() # Read left distance
set_angle(150) # Look right
d_right = read_cm() # Read right distance
if (d_left or 0) > (d_right or 0): # If left clearer
left() # Turn left briefly
else: # Else right clearer or equal
right() # Turn right briefly
time.sleep_ms(600) # Move a bit
stop() # Stop and reassess
else: # Path clear
forward() # Move forward
time.sleep_ms(600) # Move a bit
stop() # Stop and re-scan
time.sleep_ms(250) # Pace loop
External explanation
Using the official blocks keeps your code consistent with Clu‑Blocks Pro. The sonar block abstracts echo timing, and the servo block abstracts pulse widths. This lets students focus on logic: where to look, how to decide, and when to move. Clear helpers and short logs reduce confusion and make debugging friendly.
Story time
Your robot now “peeks” around with a servo eye and “listens” for obstacles. When the path is clear, it rolls forward. When something is close, it glances left and right, then turns wisely. It feels almost thoughtful.
Debugging (2)
Debugging 4.3.A – Error measurements due to angle
- Symptom: Distances glitch at extreme angles (0°/180°).
- Fix:
# Add extra settle at endpoints and avoid extreme angles when possible
set_angle(10) # Use 10° instead of 0°
time.sleep_ms(300) # More settle time near edges
Debugging 4.3.B – Servo‑ultrasonic interference
- Symptom: Timeouts or invalid readings right after moving the servo.
- Fix:
# Read only after movement settles; do not read while the servo is moving
set_angle(90) # Move to center
time.sleep_ms(220) # Wait for vibration to stop
d = read_cm() # Then measure
# Use separate 5V power for servo; always share GND with the board
Final checklist
- Ultrasonic reads with
sonar.Sonar(...).checkdist()consistently. - Servo moves with
servo.servo180_angle(pin, angle)and clamps angles safely. - 180° map collects samples at multiple angles.
- Real‑time detection triggers clear alerts.
- Movement decisions (forward/turn/stop) react calmly to scan results.
Extras
- Student tip: Tune
SAFE_CM(20–35) to match your room. Smaller values make it bolder; larger keep it careful. - Instructor tip: Have learners log
(angle, distance)pairs and discuss which angles best predict safe motion. - Glossary:
- sonar: Official block for ultrasonic distance checks.
- servo180_angle: Official block for SG90 angle control.
- threshold: A decision boundary (e.g., 25 cm for “too close”).
- Mini tips:
- Keep logs short; long prints slow robots.
- Use small delays (200–300 ms) after angle changes for consistent readings.
- If needed, send short Bluetooth logs to PC like “SCAN:90=27cm” later to monitor behavior.