June 2026 – July 2026 · Solo project

5-DOF Servo Arm

ESP32
C++
SolidWorks
Embedded Systems

A 5-DOF robotic arm built with servo motors, modular brackets, and an ESP32, featuring independently derived inverse kinematics and C++-based joint-space trajectory control for smooth motion.

Overview

The motivation behind this project stemmed from my experience on LARM, where my role was limited to the mechanical design of individual components, and I had little exposure to the electrical and software sides of robotics. This project was an attempt to close that gap and eliminate those blind spots by designing, building, and programming a robotic arm from the ground up.

Beyond closing that gap, the robot was also built to serve as a platform for testing other robotics skills I wanted to develop, such as computer vision.

The scope of the project was to build an arm capable of reliably and efficiently moving from an initial position to a user-specified target position, at a desired end-effector orientation.

Design & Approach

Since the primary goal of this project was to develop electrical and software skills rather than mechanical design skills, there was little reason to reinvent the wheel on the structural side. The arm’s main structure was built using a pre-made pack of servo brackets and screws, allowing me to focus my effort on the electronics and control software instead.

Servo brackets and hardware used to build the arm — Servo brackets and hardware used to construct the arm’s main structure.

I wasn’t planning on designing an end effector for this prototype, so I used relatively lightweight servos. However, the weight of the frame was enough that I had to replace two of my originally five MG995s with beefier 20kg servos.

MG995 servo, used for three of the five joints.

Miuzei 20kg digital servo, used for the two most heavily loaded joints.

SolidWorks drawing of the robotic arm with side views and linkage dimensions — 3D render of the robotic arm created in SolidWorks.

Specifications

Degrees of Freedom: 5
Reach: ~28 cm
Payload Capacity: NA
Actuators: 2× 20kg MS24, 3× MG995
Repeatability: TBD
Weight: TBD
Microcontroller: ESP32
Power Supply: 6V, 5A

Inverse Kinematics

The inverse kinematics of the robot were calculated by modeling it as three vectors and using the intersection of circles method (outlined in detail here: Intersection of Two Circles | Stack Overflow).

Simplified vector model of the robotic arm — Simplified model of the robot and the naming conventions used for the IK calculations.

The initial conditions of the system are the lengths \(L_1\), \(L_2\), \(L_3\), the coordinates \((r_3, z_3)\), and \(\alpha\), the angle of the end effector.

The coordinates \((r_2, z_2)\) can be calculated from the known orientation and position of \(L_3\) as follows:

\[ r_2 = r_3 - L_3\cos(\alpha) \] \[ z_2 = z_3 - L_3\sin(\alpha) \]

Now the only missing coordinate is \((r_1, z_1)\), which is found as the intersection of two circles. Circle 1 is drawn about the origin at a distance \(L_1\), and circle 2 is drawn about the previously calculated \((r_2, z_2)\) at a distance \(L_2\).

Here, \(d\) is the distance between the two circle centers (the origin and \((r_2, z_2)\)); \(w\) is the distance from the origin to the line connecting the two circle intersection points, measured along the line joining the centers; \(h\) is half the length of that connecting line between the two intersection points; and \(\beta\) is the angle of the line from the origin to \((r_2, z_2)\), measured relative to the z-axis.

Construction geometry diagram showing d, w, h, and beta — The geometric construction of d, w, h, and β used to solve for the elbow joint position via the intersection of two circles.

\[ d = \sqrt{r_2^2 + z_2^2} \] \[ w = \frac{L_1^2 - L_2^2 + d^2}{2d} \] \[ h = \sqrt{L_1^2 - w^2} \] \[ \beta = \arctan\left(\frac{r_2}{z_2}\right) \]

This yields two possible solutions for \((r_1, z_1)\): the elbow-up and elbow-down configurations, corresponding to the two intersection points of the circles.

Elbow-Up Solution

\[ r_1 = \frac{r_2 w}{d} - h\cos(\beta) \] \[ z_1 = \frac{z_2 w}{d} + h\sin(\beta) \]

Elbow-Down Solution

\[ \bar{r}_1 = \frac{r_2 w}{d} + h\cos(\beta) \] \[ \bar{z}_1 = \frac{z_2 w}{d} - h\sin(\beta) \]

Interactive inverse kinematics model. Drag the point or adjust the sliders under “Initial Conditions” to see how the joint angles respond.

Control System

The arm is controlled by an ESP32 microcontroller, programmed using C++ in Arduino IDE. I used the Servo library by Harrington.

The software is split into three layers:

Desktop GUI (Python + Pygame): the user enters an (x, y, z) target, which is sent to the ESP32.
Firmware (C++/Arduino): solves the IK for the joint angles, then interpolates between points for smooth motion.
Motor control (Servo library): generates the PWM signal for each joint’s servo.

By an informal estimate, the arm reaches commanded targets to within about 1–2 inches. Repeatability has not been measured yet.

Results & Reflections

While this project allowed me to learn some basic hardware and software skills, the project scope was very limited. Possible expansions include:

Adding an end effector
Adding a sixth degree of freedom
Using stepper motors instead of servos
Custom-designing and 3D-printing the support components