[树莓派系列] 使用WiringPi库入门模拟舵机-SG90(C和Python)

模拟舵机树莓派 SG90舵机

wybliw

关注

发布时间: 2020-10-19

丨

阅读: 4248

[TOC]

---

**难度：** ★★

**读者：** 适合有`C语言`或`Python`编程基础，对舵机和PWM等有一定了解的读者。

---

往期相关文章：

* 《 [树莓派4B-WiringPi库的安装和使用](https://www.icxbk.com/article/preview?astatus=1&aid=1707) 》
* 《 [入门WiringPi库的PWM接口](https://www.icxbk.com/article/detail?aid=1778) 》

**没有安装WiringPi的读者，可先阅读往期文章，搭建好环境。**

本文将介绍如何在树莓派4B上控制舵机运转，比如控制角度、方向等，带你入门模拟舵机。

**准备工作：**

* SG90(或其他型号)模拟舵机
* 树莓派4B
* 面包板及若干导线

---

## 舵机简介

### 模拟舵机与数字舵机

数字舵机（Digital Servo）和模拟舵机（Analog Servo）在基本的**机械结构**方面是完全一样的，主要由**马达**、**减速齿轮**、**控制电路**等组成，而数字舵机和模拟舵机的最大**区别**则体现在**控制电路**上，数字舵机的控制电路比模拟舵机的多了**微处理器**和**晶振**。模拟舵机一般通过PWM波来控制，采用并行通信，会占用较多的IO口。数字舵机是多是采用串行通信，通过指令以总线的形式控制，常见接口有UART、RS485等，比PWM波控制方式高端一点。多个数字舵机可以串连接线，每个舵机都有唯一的ID，只需将靠近控制器的舵机连到控制器上，就能控制所有舵机，占用IO口比较少。

模拟舵机是直流伺服电机控制器芯片，一般只能接收 50Hz 频率（周期 20ms）~300Hz 左右的 PWM外部控制信号，太高的频率可能就无法正常工作了。数字舵机通过MCU可以接收比 50Hz 频率更高的 PWM 外部控制信号。

---

### 模拟舵机的控制

舵机的频率一般为频率为50HZ，也就是一个20ms左右的时基脉冲，而脉冲的高电平部分一般为0.5ms-2.5ms范围。来控制舵机不同的转角。以笔者购买的180度SG90小舵机为例，假定1.5ms的脉冲宽度为舵机的0角度，那么对应的控制关系如下：

**占空比与角度位置表**

| 高电平持续时间(ms) | 占空比(周期20ms) | 舵机转过的角度(度) |
| ------------------ | ---------------- | ------------------ |
| 0.5                | 2.5%             | -90                |
| 1.0                | 5%               | -45                |
| 1.5                | 7.5%             | 0                  |
| 2.0                | 10%              | 45                 |
| 2.5                | 12.5%            | 90                 |

![](https://cf05.ickimg.com/bbsimages/202010/5b4de50349c1ead62cc38b01a9ed3067.gif)

当然，读者也可以根据自己的需要，规定0.5ms的脉冲宽度为舵机的0度位置，2.5ms脉冲宽度为舵机的180度位置。

### SG90模拟舵机实物图
![](https://cf05.ickimg.com/bbsimages/202010/e1e3b5dd9d086c837d5c09b364556e8b.png)

**拆解图：**

![](https://cf05.ickimg.com/bbsimages/202010/3783226728aa8e54b3045ab9834e6eca.png)

可以看到舵机主要由**马达**、**减速齿轮**、**控制电路**组成，其中绿色的是电位器，用来确定舵机输出轴角度位置。

---

## 模拟舵机的编程

### SG90舵机控制（C语言版）

#### 使用硬件PWM接口

树莓派默认PWM模式是`Balanced`模式，该模式下的PWM频率远大于舵机的50Hz，所以需要修改PWM的频率，此时我们需要将PWM模式切换到`Mark:Space` 模式(占空比模式)。查阅资料，树莓派的PWM时钟基础频率是19.2MHz，通过`pwmSetClock()`函数设置分频(默认是32分频)，PWM频率计算公式：

```c
pwmFrequency(Hz) = 19.2*10^6 Hz / pwmClock / pwmRange
```

为了得到更好的控制精度，这里将`range`设置为2000，即周期分为2000步，控制精度更高。要取得PWM频率为50Hz，那么divisor为：

```c
divsior = 19200*1000 / (50 * 2000) = 192
```

角度对应range值为：

| 舵机角度（度） | range值 | 占空比         |
| -------------- | ------- | -------------- |
| -90            | 50      | 50/2000=2.5%   |
| -45            | 100     | 100/2000=5%    |
| 0              | 150     | 150/2000=7.5%  |
| 45             | 200     | 200/2000=10%   |
| 90             | 250     | 250/2000=12.5% |

笔者也试了一下`divisor=1920`，`range=200`的配置方式，这样设置PWM Freq = 50Hz的，但是实际控制舵机和理论的占空比有差异，0.5ms对应range为15。

将SG90舵机连接到树莓派上，接线如下：

| SG90 Pin | 树莓派 Pin |
| -------- | ---------- |
| VCC      | +5V        |
| GND      | GND        |
| Data     | GPIO1      |

舵机连接好后，可先通过gpio命令，测试一下GPIO1，**命令如下：**

```bash

$ gpio mode 1 pwm	# 设置GPIO1为PWM输出脚
$ gpio pwm-ms		# 切换到占空比、传统模式
$ gpio pwmc 192	    # 设置时钟分频
$ gpio pwmr 2000    # 每个刻度 0.01 ms，2000 * 0.01ms = 20ms

$ gpio pwm 1 50      # 0.5 ms (-90°) => Maxrange * 占空比 = 2000 * 2.5% = 50
$ gpio pwm 1 100     # 1.0 ms (-45°) => 2000 * 5%   = 100
$ gpio pwm 1 150     # 1.5 ms (0°)   => 2000 * 7.5% = 150
$ gpio pwm 1 200     # 2.0 ms (45°)  => 2000 * 10%  = 200

```

**代码实现如下：**

```c

#include <wiringPi.h>
#include <softPwm.h>

#include <stdio.h>

// 定义控制舵机的引脚为：GPIO1
#define PWM_PIN     1

int main(void)
{
    printf("wiringPi-C PWM test program\n") ;

wiringPiSetup();

pinMode(PWM_PIN, PWM_OUTPUT);	// 设置PWM输出
    pwmSetMode(PWM_MODE_MS);		// 设置传统模式
    pwmSetClock(192);				// 设置分频
    pwmSetRange(2000);				// 设置周期分为2000步

printf("当前方向: -90度\n");
    pwmWrite(PWM_PIN, 50) ;
    delay(2000);
    printf("当前方向: -45度\n");
    pwmWrite(PWM_PIN, 100) ;
    delay(2000);
    printf("当前方向: 0度\n");
    pwmWrite(PWM_PIN, 150) ;
    delay(2000);
    printf("当前方向: 45度\n");
    pwmWrite(PWM_PIN, 200) ;
    delay(2000);
    printf("当前方向: 90度\n");
    pwmWrite(PWM_PIN, 250) ;
    delay(2000);

int i = 0;
    while(1) {
        printf("顺时针\n");
        for(i = 250; i >= 50; i--) {
            pwmWrite(PWM_PIN, i) ;
            delay(50);
        }
        printf("逆时针\n");
        for(i = 50; i <= 250; i++) {
            pwmWrite(PWM_PIN, i) ;
            delay(50);
        }
    }

return 0 ;
}

```

将代码保存到`pwm_sg90.c`文件，编译、运行。

```bash
# 编译
gcc pwm_sg90.c -o pwm_sg90 -lwiringPi

# 运行
sudo ./pwm_sg90

```

**效果如下图：**

![](https://cf05.ickimg.com/bbsimages/202010/4804db4b6b08814e700c40ada8fb23aa.gif)

---

#### 使用软件PWM接口

由于硬件PWM接口只有4个，有时可能会被其他资源占用，此时就需要使用软件模拟PWM接口，wiringpi的软件PWM可以在任意一个GPIO口上实现。接口函数介绍详见 《 [入门WiringPi库的PWM接口(C和Python)]() 》

根据公式：`PWMfreq = 1 x 10^6 / (100 x range)` ，要得到PWM频率为50Hz，则range为200，即周期分为200步，控制精度相比硬件PWM较低。

**舵机接线：**

| 舵机 Pin | 树莓派 Pin |
| -------- | ---------- |
| VCC      | +5V        |
| GND      | GND        |
| Data     | GPIO0      |

**代码实现如下：**

```c
#include <wiringPi.h>
#include <softPwm.h>

#include <stdio.h>

#define PWM_PIN     0

int main(void)
{
    printf("wiringPi-C PWM test program\n") ;

// 初始化
    wiringPiSetup();

pinMode(PWM_PIN, OUTPUT);
    softPwmCreate(PWM_PIN, 0, 200);		// 设置周期分为200步

printf("当前方向: -90度\n");
    softPwmWrite(PWM_PIN, 5);
    delay(1000);
    printf("当前方向: -45度\n");
    softPwmWrite(PWM_PIN, 10);
    delay(1000);
    printf("当前方向: 0度\n");
    softPwmWrite(PWM_PIN, 15);
    delay(1000);
    printf("当前方向: 45度\n");
    softPwmWrite(PWM_PIN, 20);
    delay(1000);
    printf("当前方向: 90度\n");
    softPwmWrite(PWM_PIN, 25);
    delay(1000);
    int i = 0;
    while(1) {
        printf("顺时针\n");
        for(i = 25; i >= 5; i--) {
            softPwmWrite(PWM_PIN, i) ;
            delay(20);
        }
        printf("逆时针\n");
        for(i = 5; i <= 25; i++) {
            softPwmWrite(PWM_PIN, i) ;
            delay(20);
        }
    }

return 0 ;
}

```

将代码保存到`pwms_sg90.c`文件，编译、运行。

```bash
# 编译
gcc pwms_sg90.c -o pwms_sg90 -lwiringPi

# 运行(可不加sudo)
sudo ./pwms_sg90

```

**效果如下图：**

![](https://cf05.ickimg.com/bbsimages/202010/da0728fbbe944560c34e37101dc1abbf.gif)

由于PWM周期只有200步，控制细分度较低。

---

### SG90舵机控制（Python版）

#### 使用硬件PWM接口

舵机接线、PWM频率设置等和C语言版类似，具体**代码如下：**

```python
#!/usr/bin/env python
# coding=utf-8

# 查看WiringPi源文件获得
INPUT       = 0
OUTPUT      = 1
PWM_OUTPUT  = 2
GPIO_CLOCK  = 3
SOFT_PWM_OUTPUT = 4
SOFT_TONE_OUTPUT= 5
PWM_TONE_OUTPUT = 6

LOW     = 0
HIGH    = 1

PWM_MODE_MS 	= 0
PWM_MODE_BAL    = 1

import wiringpi

# 设置舵机控制引脚为GPIO1
PWM_PIN = 1

wiringpi.wiringPiSetup()				# WiringPi初始化
wiringpi.pinMode(PWM_PIN, PWM_OUTPUT)  	# 设置引脚为PWM模式
wiringpi.pwmSetMode(PWM_MODE_MS)		# 修改PWM为传统模式
wiringpi.pwmSetClock(192)				# 设置分频系数
wiringpi.pwmSetRange(2000)				# 设置周期步长(占空比范围)

print "当前方向: -90度\n"
wiringpi.pwmWrite(PWM_PIN, 50)
wiringpi.delay(1000)

print "当前方向: -45度\n"
wiringpi.pwmWrite(PWM_PIN, 100)
wiringpi.delay(1000)

print "当前方向: 0度\n"
wiringpi.pwmWrite(PWM_PIN, 150)
wiringpi.delay(1000)

print "当前方向: 45度\n"
wiringpi.pwmWrite(PWM_PIN, 200)
wiringpi.delay(1000)

print "当前方向: 90度\n"
wiringpi.pwmWrite(PWM_PIN, 250)
wiringpi.delay(1000)

while 1:
    print "顺时针\n"
    for duty in range(250, 50, -1):
        wiringpi.pwmWrite(PWM_PIN, duty)
        wiringpi.delay(10)

print "逆时针\n"
    for duty in range(50, 250):
        wiringpi.pwmWrite(PWM_PIN, duty)
        wiringpi.delay(10)

```

将代码保存到`pwm_sg90.py`文件， 输入以下指令运行程序：

```bash
sudo python pwm_sg90.py
```

最终效果和C语言版本一样，这里就不再贴图了。

---

#### 使用软件PWM接口

参数设置介绍具体可见上文C语言版本。

**代码如下：**

```python
#!/usr/bin/env python
# coding=utf-8

# 查看WiringPi源文件
INPUT       = 0
OUTPUT      = 1
PWM_OUTPUT  = 2
GPIO_CLOCK  = 3
SOFT_PWM_OUTPUT = 4
SOFT_TONE_OUTPUT= 5
PWM_TONE_OUTPUT = 6

LOW     = 0
HIGH    = 1

PWM_MODE_MS 	= 0
PWM_MODE_BAL    = 1

import wiringpi

# 设置舵机控制引脚为 GPIO0
PWM_PIN = 0

wiringpi.wiringPiSetup()
wiringpi.pinMode(PWM_PIN, OUTPUT)			# 设置引脚模式：输出
wiringpi.softPwmCreate(PWM_PIN, 0, 200)		# 设置PWM频率50Hz

print "当前方向: -90度\n"
wiringpi.softPwmWrite(PWM_PIN, 5)
wiringpi.delay(1000)

print "当前方向: -45度\n"
wiringpi.softPwmWrite(PWM_PIN, 10)
wiringpi.delay(1000)

print "当前方向: 0度\n"
wiringpi.softPwmWrite(PWM_PIN, 15)
wiringpi.delay(1000)

print "当前方向: 45度\n"
wiringpi.softPwmWrite(PWM_PIN, 20)
wiringpi.delay(1000)

print "当前方向: 90度\n"
wiringpi.softPwmWrite(PWM_PIN, 25)
wiringpi.delay(1000)

while 1:
    print "顺时针\n"
    for duty in range(25, 5, -1):
        wiringpi.softPwmWrite(PWM_PIN, duty)
        wiringpi.delay(20)

print "逆时针\n"
    for duty in range(5, 25):
        wiringpi.softPwmWrite(PWM_PIN, duty)
        wiringpi.delay(20)

```

将代码保存到`pwms_sg90.py`文件，输入以下指令运行程序（可以不加sudo）：

```bash
sudo python pwms_sg90.py
```

最终效果和C语言版一样，这里就不再贴图了。

由于笔者手里没有示波器，设置的PWM波形无法得到证实，目前只能通过查阅资料来计算PWM频率，然后测试舵机。后期有示波器后再研究一下设置的参数。

---

## 参考文献

* [Control Hardware PWM frequency](https://raspberrypi.stackexchange.com/questions/4906/control-hardware-pwm-frequency)

* [Raspberry Pi](https://projects.drogon.net/raspberry-pi/)

* [wiringPi库之PWM参数试验](https://zhuanlan.zhihu.com/p/254467548)

== end ==

原创作品，未经权利人授权禁止转载。详情见转载须知。举报文章

wybliw

关注

登录后可评论，请登录或注册

xbk_248534 308天前...

请问我用c++代码调用wiringPi库后，catkin_make编译出现/usr/lib/gcc/ssrch64-linux-gnu/7/../../../../lib/libwiringPi.so:undefined reference to 'shm_open' 是为什么？不能用g++编译，因为c++代码中要调用ros.h库

0 回复