Product Support

Catalogue

Servo

BLDC

Gear Motor

Custom Drive

How to Write a Servo Motor Control Program: A Complete Step-by-Step Guide

Published 2026-04-04

This article provides a complete, practical guide to writing a program that controls a standardservomotor. You will learn the exact code structure, the critical PWM signal parameters, and how to implement a working example using common microcontrollers. No brand-specific or proprietary platforms are required; the principles apply universally.

01Core Concept: The One Essential Rule forservoControl

Aservomotor’s position is determined solely by the width of a repeating pulse (Pulse Width Modulation, PWM).To write any servo control program, you must generate a 50 Hz signal (period = 20 milliseconds) and vary the high pulse width between 1.0 ms and 2.0 ms.This single rule is the foundation of all standard servo programming.

1.0 ms pulse → 0 degrees(full counter-clockwise)

1.5 ms pulse → 90 degrees(center position)

2.0 ms pulse → 180 degrees(full clockwise)

02What You Need Before Writing the Code

To follow this guide, prepare the following components (generic descriptions, no brand names):

One standard 5V servo motor (3-wire: power, ground, signal)

One microcontroller board (any common 5V logic board)

One external 5V power supply (if the servo draws high current)

Jumper wires and a breadboard

03Step-by-Step Program Writing Process

Step 1: Connect the Hardware Correctly

Before any code, establish the physical connections:

Servo Wire	Connect to
Brown/Black (Ground)	GND on microcontroller
Red (Power, 5V)	5V output (or external 5V supply common ground)
Orange/Yellow (Signal)	A PWM-capable digital pin (e.g., pin 9)

Common case example:A beginner often connects the servo’s power directly to the microcontroller’s 5V pin. When the servo moves, the board resets due to current surge. The fix: use an external 5V supply (e.g., 4xAA batteries) with shared ground.

Step 2: Write the Initialization Code

Every servo program must first set up the PWM signal parameters. Below is a generic code structure (adaptable to any C-like environment):

// 1. Define constants (do not change these values) #define SERVO_PIN 9 #define PWM_FREQUENCY_HZ 50 // 50 Hz = 20 ms period #define PULSE_MIN_US 1000 // 1.0 ms for 0° #define PULSE_MID_US 1500 // 1.5 ms for 90° #define PULSE_MAX_US 2000 // 2.0 ms for 180°// 2. Setup function (runs once at power-on)void setup() { // Configure the pin as output pinMode(SERVO_PIN, OUTPUT); // Configure PWM hardware for 50 Hz and 1.5 ms initial pulse (center position) configurePWM(SERVO_PIN, PWM_FREQUENCY_HZ, PULSE_MID_US); delay(1000); // Allow servo to stabilize }// 3. Helper: Convert angle (0-180) to pulse width in microsecondsint angleToPulse(int angle) { // Linear mapping: angle 0 -> 1000us, angle 180 -> 2000us return PULSE_MIN_US + (angle(PULSE_MAX_US - PULSE_MIN_US) / 180); }

Step 3: Write the Core Control Function

The main logic that moves the servo to a specific angle:

// Move servo to a given angle (0 to 180) and hold that positionvoid setServoAngle(int angle) { // Clamp angle within valid range if (angle 180) angle = 180; int pulseWidthUs = angleToPulse(angle);// Generate a single 20 ms cycle with the specified high pulsegeneratePulse(SERVO_PIN, pulseWidthUs); // High for pulseWidthUs microseconds delayMicroseconds(pulseWidthUs); digitalWrite(SERVO_PIN,LOW); delay(20 - (pulseWidthUs / 1000.0)); // Remainder of 20 ms period }

Note:ThegeneratePulse()abstraction represents direct hardware register manipulation. In real implementations, you would use the microcontroller’s built-in PWM peripheral or a software bit-banging method.

Step 4: Write a Complete Working Example

Below is a full program that sweeps the servo from 0° to 180° and back, with delays to observe movement. This example uses a common bit-banging approach that works on any digital pin.

// Full working code (no library required, platform-agnostic) int servoPin = 9; unsigned long previousMicros = 0; int currentAngle = 0; int stepDirection = 1; // 1 = increasing angle, -1 = decreasing void setup() { pinMode(servoPin, OUTPUT); // Start at 90° (center) currentAngle = 90; } void loop() { unsigned long currentMicros = micros(); // Refresh servo position every 20 ms (50 Hz) if (currentMicros - previousMicros >= 20000) { previousMicros = currentMicros; // Send the PWM pulse for the current angle int pulseWidth = 1000 + (currentAngle 1000 / 180); // 1000us to 2000us digitalWrite(servoPin, HIGH); delayMicroseconds(pulseWidth); digitalWrite(servoPin, LOW); // Change angle for sweeping effect (optional) currentAngle += stepDirection; if (currentAngle >= 180) { currentAngle = 180; stepDirection = -1; delay(500); // Pause at extreme } else if (currentAngle

Step 5: Testing and Debugging Common Issues

Problem	Most Likely Cause	Verification Method
Servo does not move at all	Missing ground connection	Use a multimeter to check continuity between servo ground and microcontroller ground
Servo jitters or vibrates	Pulse timing is inconsistent	Ensure your delay between pulses is exactly 20 ms (±1 ms tolerance)
Servo moves only to extreme ends	Wrong pulse width range	Measure actual pulse width with an oscilloscope or logic analyzer
Servo moves opposite to expected	Signal and power wires swapped	Double-check wire colors against datasheet

Common case example: A user writes code that sends pulses every 5 ms instead of 20 ms. The servo receives signals too fast, overheats, and vibrates. The fix is to ensure exactly 20 ms between the start of each pulse.

04Advanced: Writing a Non-Blocking Servo Program

For projects that need to multitask (e.g., read sensors while moving the servo), use a state machine without delay():

unsigned long lastPulseTime = 0;
int targetAngle = 90;
int currentPulseWidth = 1500;  // start at center
void updateServoNonBlocking() {
    unsigned long now = micros();
    if (now - lastPulseTime >= 20000) {  // 20 ms elapsed
        lastPulseTime = now;
        // Calculate target pulse width from targetAngle
        int targetPulse = 1000 + (targetAngle * 1000 / 180);
        // Gradually move to target (smoother motion)
        if (currentPulseWidth  targetPulse) currentPulseWidth--;
        digitalWrite(servoPin, HIGH);
        delayMicroseconds(currentPulseWidth);
        digitalWrite(servoPin, LOW);
    }
}

CallupdateServoNonBlocking() repeatedly in your main loop.

05Summary: The Two Critical Points to Repeat

1. The servo position depends exclusively on the high pulse width – 1.0 ms (0°), 1.5 ms (90°), 2.0 ms (180°). Nothing else changes the angle.

2. The pulse must repeat every 20 ms (50 Hz) – Any deviation causes jitter, overheating, or no movement.

06Actionable Recommendations to Strengthen Your Results

Start with a minimal test program that only sets the servo to 90° and holds. Verify with a protractor that the horn is at center. This confirms your timing is correct before adding sweeps or sensor logic.

Use a logic analyzer or oscilloscope to measure the actual signal on the signal pin. Compare the measured pulse width against your code’s intended values. This is the most reliable debugging method.

Always use a separate power supply for servos that draw more than 200 mA. Sharing power with the microcontroller causes resets and erratic behavior. Connect all grounds together.

Add a 100-470 µF electrolytic capacitor across the servo’s power and ground terminals near the servo. This reduces electrical noise and stabilizes the control signal.

Document your pulse timing constants in microseconds directly in the code. This helps when you revisit the project months later.

By following this guide, you can write a reliable servo control program on any microcontroller platform without relying on pre-made libraries. The core PWM principle remains identical across all systems. Implement the non-blocking example for real-time applications, and always validate your pulse widths with measurement tools.

Update Time：2026-04-04

Back Prev Back Next