12 - Servo Control Application

This chapter describes the servo control application example — servo_ctrl — on the Pico-G1 expansion board. The application demonstrates how to achieve precise servo angle control through the PWM interface, supporting angle setting, sweep operation, centering, and other functions, and how to display the servo status on a TFT screen. It is a practical example for learning PWM control and servo programming.

The application source code is located in the SDK directory source/app/12_servo_ctrl/ and provides a complete PWM servo control implementation.

1 Application Overview

1.1 Features

  • PWM servo control: achieves precise servo angle control through the PWM interface
  • Angle range: supports the standard 0°~180° servo angle range
  • High-precision positioning: angle control accuracy ±1°
  • Multiple running modes: supports angle setting, sweep operation, centering, and more
  • Real-time status display: shows the angle, pulse width, and running state on the TFT screen

1.2 Technical Specifications

ParameterValue
Servo typeStandard 9 g micro servo (compatible with MG996R)
Control signalPWM (50 Hz period)
Pulse-width range0.5 ms~2.5 ms (corresponding to 0°~180°)
Operating voltage4.8V~6V (5V recommended)
Control accuracy±1°
Refresh rate50 Hz (20 ms period)
Control interfaceGPIO PWM output

1.3 Test Case List

indexNameTest commandExpected result (success)Possible causes of failure
1Angle control./servo_ctrlServo moves to the specified angle, TFT shows the angleWrong PWM config, insufficient supply
2Accuracy testSet 90 degreesServo positions precisely at the middleWrong pulse-width calculation, mechanical jitter
3Sweep testStart sweep modeServo moves back and forth between 0°~180°Inaccurate PWM frequency
4Load testAdd a mechanical loadServo stays stable, angle unchangedInsufficient supply, insufficient torque

1.4 Directory Structure

source/app/12_servo_ctrl/
├── Makefile              # Build script
├── main.c                # Main program
├── servo.c               # Servo driver implementation
├── servo.h               # Servo driver header
├── pwm_hal.c             # PWM HAL layer implementation
├── pwm_hal.h             # PWM HAL layer header
├── spi_hal.c             # SPI HAL layer implementation
├── spi_hal.h             # SPI HAL layer header
├── st7789.c              # ST7789 driver implementation
├── st7789.h              # ST7789 driver header
├── font8x16.h            # 8×16 ASCII bitmap font
└── README.md             # Documentation

2 Hardware Connection

2.1 Pin Definitions

SignalOn-board GPIODescription
PWMGPIO6_0PWM control signal output
VCC5VServo supply (4.8V~6V)
GNDGNDGround

2.2 Hardware Circuit

Servo wiring diagram:

     Pico-G1                   Servo Motor
  ┌───────────┐              ┌──────────────┐
  │           │              │              │
  │ GPIO6_0 ──┼────── PWM ───┤ ORANGE       │
  │           │              │              │
  │     5V ────┼─────────────┤ RED          │
  │           │              │              │
  │     GND ───┼─────────────┤ BROWN        │
  └───────────┘              └──────────────┘

Servo power supply

Servos draw relatively large currents. An external 5V supply is recommended; powering the servo from the board can cause voltage instability.

2.3 PWM Control Principle

Servo angle vs. pulse width:

0°   → 0.5 ms  high level
90°  → 1.5 ms  high level (center)
180° → 2.5 ms  high level

PWM period: 20 ms (50 Hz)
Duty cycle = (pulse width / 20 ms) × 100%

3 Build and Deployment

3.1 Build the Application

export PATH=$PATH:<SDK>/tools/linux/toolchains/arm-gcc12.2.0-linux-uclibceabi/bin
cd <SDK>/source/app/12_servo_ctrl
make

3.2 Run the Application

scp servo_ctrl root@<board_ip>:/usr/bin/
ssh root@<board_ip> '/usr/bin/servo_ctrl'

3.3 Expected Output

Console output

/mnt # ./servo_ctrl
[pca] pad 复用：I2C3(4_1/4_2)->func2，GPIO4_5(OE)->func5
[pca] pad 0x100C0010 -> 0x00001002
[pca] pad 0x100C0014 -> 0x00001002
[pca] pad 0x100C0020 -> 0x00001005
[pca] init PCA9685 (PWM 50Hz)...
[pca] MODE1=0x00 (reset OK)
[pca] @ /dev/i2c-3 addr 0x41 init OK, PWM 50 Hz
[pca] OE 已使能，开始接受命令
命令:
  s <ch 0..15> <us>  a <us>  c  o  q  h
脉宽: 500us(0°) / 1500us(90°) / 2500us(180°)
> ch 0 1500
[pca] 全部居中 (1500 us)

4 Servo Control Principles

4.1 PWM Timing

Standard servo PWM timing:

  • Period: 20 ms (50 Hz)
  • Pulse-width range: 0.5 ms ~ 2.5 ms
  • Update rate: 50 Hz recommended, no more than 100 Hz

4.2 Angle Calculation

Angle to pulse-width conversion:

int angle_to_pulse_width(int angle)
{
    // Angle range: 0~180 degrees
    // Pulse-width range: 500~2500 microseconds
    return 500 + (angle * 2000 / 180);
}

Pulse width to duty-cycle conversion:

float pulse_width_to_duty_cycle(int pulse_width_us)
{
    // PWM period: 20000 microseconds
    return (pulse_width_us / 20000.0f) * 100.0f;
}

4.3 PWM Generation

int servo_set_pwm(int angle)
{
    // Compute the pulse width
    int pulse_width_us = angle_to_pulse_width(angle);

    // Compute the duty cycle (period 20000 microseconds)
    int period_ns = 20000000;  // 20 ms
    int duty_ns = pulse_width_us * 1000;

    // Set the PWM
    pwm_set_config(GPIO6_0, period_ns, duty_ns);

    return pulse_width_us;
}

5 Servo Control Modes

5.1 Basic Control Functions

// Set the angle (0~180°)
void servo_set_angle(int angle)
{
    if (angle < 0) angle = 0;
    if (angle > 180) angle = 180;

    int pulse_width = servo_set_pwm(angle);
    printf("[Servo] 角度: %d° 脉宽: %.2fms\n", angle, pulse_width / 1000.0f);
}

// Center the servo (90°)
void servo_center(void)
{
    servo_set_angle(90);
}

// Set the pulse width (microseconds)
void servo_set_pulse_width(int pulse_width_us)
{
    int period_ns = 20000000;  // 20 ms
    int duty_ns = pulse_width_us * 1000;

    pwm_set_config(GPIO6_0, period_ns, duty_ns);
}

5.2 Sweep Operation

void servo_sweep(int start_angle, int end_angle, int step, int delay_ms)
{
    int direction = (start_angle < end_angle) ? 1 : -1;
    int current_angle = start_angle;

    while (1) {
        servo_set_angle(current_angle);

        current_angle += direction * step;

        // Boundary detection
        if (current_angle >= end_angle || current_angle <= start_angle) {
            direction *= -1;  // Reverse
            current_angle = (direction > 0) ? start_angle : end_angle;
        }

        usleep(delay_ms * 1000);
    }
}

5.3 Angle Limits

typedef struct {
    int min_angle;
    int max_angle;
} servo_limits_t;

void servo_set_angle_limited(int angle, servo_limits_t *limits)
{
    if (angle < limits->min_angle) {
        angle = limits->min_angle;
    }
    if (angle > limits->max_angle) {
        angle = limits->max_angle;
    }

    servo_set_angle(angle);
}

6 Key Programming Points

6.1 PWM Initialization

int pwm_init_for_servo(void)
{
    // Export the PWM
    pwm_export(GPIO6_0);

    // Set the period (20 ms = 20000000 ns)
    int period_ns = 20000000;
    pwm_set_period(GPIO6_0, period_ns);

    // Initialize to the center position (1.5 ms)
    pwm_set_duty_cycle(GPIO6_0, 1500000);  // 1.5 ms

    // Enable the PWM
    pwm_enable(GPIO6_0);

    return 0;
}

6.2 Angle Precision Control

// High-precision angle control (supports decimals)
void servo_set_angle_precise(float angle)
{
    if (angle < 0.0f) angle = 0.0f;
    if (angle > 180.0f) angle = 180.0f;

    // Compute the pulse width precisely
    int pulse_width_us = (int)(500 + angle * 2000.0f / 180.0f);

    servo_set_pulse_width(pulse_width_us);
}

6.3 Speed Control

void servo_move_with_speed(int target_angle, int step_delay_us)
{
    int current_angle = get_current_angle();
    int direction = (target_angle > current_angle) ? 1 : -1;

    while (current_angle != target_angle) {
        current_angle += direction;
        servo_set_angle(current_angle);
        usleep(step_delay_us);
    }
}

7 Troubleshooting

ProblemPossible causeSolution
Servo jittersInaccurate PWM frequencyCalibrate the PWM frequency to 50 Hz
Angle deviationWrong pulse-width calculationRecalibrate the pulse-width/angle relation
Servo unresponsiveWrong PWM connectionCheck the GPIO6_0 output
Servo overheatsSupply voltage too highCheck the supply voltage (5V recommended)
Insufficient rangeMechanical limits or improper pulse-width rangeCheck the mechanics, adjust the pulse-width range
Weak servoInsufficient supply currentUse an external power supply

Servo usage tips

  • Power capacity: make sure the supply can provide enough current (at least 500 mA per servo)
  • Mechanical design: avoid driving the servo beyond its mechanical limits
  • PWM precision: use hardware PWM for more stable control
  • Regular calibration: recalibrate the center position every 6 months

8 Advanced Features

8.1 Smooth Motion

void servo_smooth_move(int start_angle, int end_angle, int total_time)
{
    int angle_diff = abs(end_angle - start_angle);
    int steps = angle_diff / 2;  // One step per 2 degrees
    int delay = total_time * 1000 / steps;

    int direction = (end_angle > start_angle) ? 1 : -1;
    int current_angle = start_angle;

    for (int i = 0; i <= steps; i++) {
        servo_set_angle(current_angle);
        current_angle += direction * 2;
        usleep(delay);
    }

    servo_set_angle(end_angle);  // Ensure the target angle is reached
}

8.2 Multi-Servo Control

typedef struct {
    int pwm_pin;
    int current_angle;
} servo_channel_t;

servo_channel_t servos[3] = {
    {GPIO6_0, 90},  // Servo 1
    {GPIO6_1, 90},  // Servo 2
    {GPIO6_2, 90},  // Servo 3
};

void multi_servo_set_angles(int *angles, int num_servos)
{
    for (int i = 0; i < num_servos; i++) {
        int pulse_width = angle_to_pulse_width(angles[i]);
        pwm_set_duty_cycle(servos[i].pwm_pin, pulse_width * 1000);
        servos[i].current_angle = angles[i];
    }
}

8.3 Position Feedback

// Read the current servo angle (via the potentiometer)
int servo_read_feedback(int adc_channel)
{
    int adc_value = read_adc(adc_channel);

    // Convert the ADC value to an angle (assuming 0~180 degrees maps to 0~4095 ADC)
    int angle = adc_value * 180 / 4095;

    return angle;
}

// Closed-loop position control
void servo_goto_position(int target_angle, int adc_channel)
{
    int current_angle = servo_read_feedback(adc_channel);
    int error = target_angle - current_angle;

    while (abs(error) > 2) {  // 2-degree tolerance
        int correction = error > 0 ? 1 : -1;
        servo_set_angle(current_angle + correction);

        usleep(10000);  // Wait for the servo to respond
        current_angle = servo_read_feedback(adc_channel);
        error = target_angle - current_angle;
    }
}

8.4 Servo Calibration

typedef struct {
    int min_pulse_width;
    int max_pulse_width;
    int min_angle;
    int max_angle;
} servo_calibration_t;

servo_calibration_t servo_calibration = {
    .min_pulse_width = 500,   // 0.5 ms
    .max_pulse_width = 2500,  // 2.5 ms
    .min_angle = 0,
    .max_angle = 180,
};

int servo_calibrated_angle(int angle)
{
    // Compute the pulse width from the calibration data
    int pulse_range = servo_calibration.max_pulse_width - servo_calibration.min_pulse_width;
    int angle_range = servo_calibration.max_angle - servo_calibration.min_angle;

    int pulse_width = servo_calibration.min_pulse_width +
                      (angle - servo_calibration.min_angle) * pulse_range / angle_range;

    return pulse_width;
}

9 References