---

Context

• Goal: Automated plug insertion system for robotic charging
• Full-stack ownership: Designed and built entire project including mechanical hardware, electronics, and software
• CAD: SOLIDWORKS assembly with custom parts (base_housing, camera_bracket, gear_carrier, ring_gear)
• Codebase: ~1,822 lines Python (vision/FSM) + ~257 lines C++ (motor control)

---

Core Problem

• Plug insertion requires sub-millimeter alignment precision
• Real-time motor control cannot run reliably on Linux (non-deterministic)
• No force sensor available for contact detection
• GPU unavailable on Raspberry Pi for neural network inference

Why this is hard: Sub-millimeter precision requires real-time determinism that Linux cannot guarantee, while contact detection typically needs force sensors unavailable in this design.

Key Insight

Mechanical contact can be detected without force sensors by monitoring position variance—when the actuator pushes against a fixed object, motor oscillation increases position instability.

Implementation:

• RPi4: Vision processing and high-level state machine (Python)
• Arduino Mega 2560: Deterministic real-time motor control (C++)
• Variance threshold: 1.5 (3x normal steady-state variance of <0.5)

---

Approach

Phase 1: Hardware Design

4-axis motion system:

• X, Z axes: Linear steppers with 8mm pitch lead screws (~5μm/step resolution)
• Y axis: Linear stepper with 2mm pitch lead screw (~1.25μm/step resolution)
• Pi axis: Rotational servo (STS-3215) for angular alignment

Design decision: Y-axis uses finer pitch (2mm vs 8mm) for higher vertical precision at the cost of slower movement.

Phase 2: Vision Pipeline

Template matching (OpenCV cv2.TM_CCOEFF_NORMED):

• 9 port templates + 3 charger templates
• Threshold: 0.70-0.80
• Detection time: <50ms per frame
• Distance calculation: 1.36 * (156 - port_width) pixels to mm

Design decision: Template matching over YOLOv8 as primary method. YOLOv8 models available as backup (ONNX 12MB, TFLite 3.2MB) but template matching runs faster on RPi without GPU acceleration.

Phase 3: Control System

10-state FSM for charging sequence: WAIT → XY_PREPARE → Z_APPROX → MOVE_TO_CONTACT → XY_PRECISE → INSERT → CHARGE → CHARGING → DISCHARGE → DISCHARGE_MOVE_BACK

Failure recovery: Contact detection timeout triggers retract and retry alignment. Max 3 retries before abort.

Binary serial protocol:

• 56-byte data packets (Arduino → RPi)
• 6-byte command packets (RPi → Arduino)
• 115200 baud, 10 Hz update rate (100ms)

Contact detection via variance analysis:

• 100-sample position buffer (10 seconds at 10 Hz)
• Normal steady-state variance: <0.5
• Variance threshold: 1.5 (3x normal, set to avoid false positives)
• When Z-axis variance exceeds threshold, motor oscillation against obstruction indicates contact

---

Tradeoffs

Decision	Rationale	Tradeoff
RPi + Arduino split	Real-time motor control vs vision processing separation	Serial communication complexity
Template matching primary	Fast processing on RPi without GPU	Requires manual template creation
Y-axis 2mm pitch	Higher precision for vertical alignment	Slower Y movement
Variance-based contact detection	No force sensor needed	Requires threshold tuning
Binary serial protocol	Efficient parsing, deterministic size	Less flexible than text

---

Results

Validation (100+ consecutive test cycles):

• Success rate: >95% (failures recovered on retry)
• Average docking time: 15 seconds (vs ~30 seconds manual insertion)
• Position accuracy: ±0.5mm (port tolerance ±2mm, 4x margin)

Testing infrastructure:

• Unit tests: test_agent.py, test_detector.py (FSM and vision validation)
• Regression logs: success.log, fail.log for failure pattern analysis

---

Key Takeaway

• Controller separation enables reliable real-time motor control while maintaining flexible vision processing
• Variance-based contact detection eliminates need for force sensors through statistical analysis
• Template matching provides sufficient accuracy for precision alignment without GPU requirements
• Full mechanical + electrical + software integration demonstrates end-to-end system ownership