树莓派(4B)ubuntu之ROS输出PWM

树莓派ubuntu之ROS控制输出PWM

• 简单介绍
• Wiring Pi方案
• • Wiring Pi安装
  • Wiring Pi硬件输出PWM
  • Wiring Pi软件输出PWM
• Pigpio方案
• • Pigpio安装
  • • 直接安装
    • make安装
  • Pigpio输PWM
  • C++调用Python
• 参考

简单介绍

由于项目需要，需要使用到树莓派输出PWM控制舵机，因此采用了几种不同的方案，效果都不太一样，在此记录一下。

Wiring Pi方案

因为使用的是树莓派的引脚来输出PWM所以要使用控制树莓派引脚的一些库，WiringPi只是其中的一种，此外还有gpiozero、pigpio等库，据说pigpio是可以在ubuntu20.04下正常使用的库，言外之意就是其他两个库多多少少可能有点问题，但是我在使用WiringPi时大部分功能也是可以使用的，有人说不支持PI4B，但是我用的还行。。。

Wiring Pi安装

sudo apt-get install wiringpi
//对于树莓派2019年5月之后的系统（早于之前的可不用执行），可能需要进行升级：
wget https://project-downloads.drogon.net/wiringpi-latest.deb
sudo dpkg -i wiringpi-latest.deb
gpio -v
// 运行gpio -v会出现2.52版本，如果没有出现说明安装出错

这里先简单介绍一下硬件PWM和软件PWM分别是什么意思？
硬件PWM的意思就是在下图的引脚里面专门红色线标出来的两个PWM口，这是由树莓派自己提供的可输出PWM的口，不用具体写繁琐的代码。
软件PWM的意思就是，大家都知道PWM的原理。自行卡好时间输出高低电平就可以了，所以软件PWM就是任意可以控制Pin口都可以，由Wiring Pi库自行写好的函数进行控制PWM输出。

但是对于我们来说写代码的过程差不多都一样都是调用函数，反正软件PWM有库函数也帮你写好了。自己只管调用就好了。

这里虽然硬件上树莓派提供了两个PWM通道，但是经过测试当PWM0运行是两个引脚对地输出的波形是一样的，即当使用PWM0时，GPIO 1和GPIO 26 输出的波形是一样的。

WiringPi Pin	BCM Pin	Physical Pin	PWM0、PWM1
GPIO 1	GPIO 18	Pin 12	PWM0
GPIO 26	GPIO 12	Pin 32	PWM0
GPIO 23	GPIO 13	Pin 33	PWM1
GPIO 24	GPIO 19	Pin 35	PWM1

可以看到wiringpi库有三种引脚编号方式，分别为：
BCM编号方式——就是使用芯片的GPIO引脚编号。

wiringpi库编号方式——使用wiringpi库自己规定的编号方式。

排针引脚编号方式——按照树莓派上20*2排针的引脚编号方式。

这里以wiringPi-Python库为例，提供了四种配置函数：

wiringPiSetup ——> wiringpi编号

wiringPiSetupGpio ——> BCM编号

wiringPiSetupPhys ——> 排针物理编号

wiringPiSetupSys ——> BCM编号，使用的/sys/class/gpio下的映射
参考的这里

wiringpi编号

BCM编号

对应关系

Wiring Pi硬件输出PWM

硬件PWM可以参考这里和这里或者这里
在这里先举一个简单的硬件输出PWM的例子，led呼吸灯。

#include 
#include 
#include 
// 设置GPIO1为LED灯的控制引脚
#define PWM_PIN     1//可以换成23 24 26
int main(void)
{
    int bright ;
    printf("wiringPi-C PWM test programn") ;
    // 配置gpio
    wiringPiSetup();//使用wiringpi编号
    // 配置GPIO1为PWM模式
    pinMode(PWM_PIN, PWM_OUTPUT);
    while(1) {
        for(bright = 0 ; bright < 1000; bright++) {
            // bright为 0 ~ 1000，最大是1023
            pwmWrite(PWM_PIN, bright);
            delay(1) ;
        }
        for(bright = 1000; bright >= 0; bright--) {
            pwmWrite(PWM_PIN, bright);
            delay(1) ;
        }
    }
    return 0 ;
}

将代码保存到 led_pwm.c 文件，然后执行编译命令：

下面展示一些 内联代码片。

gcc led_pwm.c -o led_pwm -lwiringPi

运行程序

sudo ./led_pwm

使用硬件PWM接口需要包含头文件：#include

pwmSetClock (int divisor)
参数： divisor：设置PWM时钟的分频。范围：2 ~ 4095。
注： PWM基础时钟19.2MHz，WiringPi库在初始化时，默认divisor值是32，因此默认PWM时钟就为PWMfreq = 19.2 x 1000 x 1000 / 32 = 600KHz。

pwmSetMode (int mode)

参数： mode：PWM发生器能在2中模式下工作，Balanced 和 Mark:Space 模式(占空比模式)，对应的设置参数： PWM_MODE_BAL 与 PWM_MODE_MS 。 Mark:Space 是传统PWM模式，树莓派默认PWM工作在 Balanced 模式。需要重新设置占空比，就要设置为Mark:Space 模式。

pwmSetRange (int range)

参数： range：用来设置PWM的周期，默认值是1024。计算方式：比如600KHz的PWM时钟，
range = (600 x 1000Hz) / PWMfreq

pwmWrite (int pin, int value)
参数：
pin： 硬件PWM引脚编号(在WiringPi中的编号)，将在该引脚上产生PWM波。
value： 设置占空比，value取值范围：0 ~ range，默认范围：0-1023。因为一个周期分为range等份，因此占空范围0~range。

到这里基本上你运行上边的那个生成的可执行文件在对应的pin口是可以检测到对应的PWM了。
但是我的要求不仅仅如此，而是在ROS里进行控制，可随时改变占空比，用来控制舵机。
当我把上边的程序写道我的话题或者服务里是，问题就出现了，具体的报错是因为没有权限控制指定的pin口，

No access to /dev/mem. Try running as root!

而运行上边的那个生成的可执行文件并没有出现这样的问题是因为前面加了sudo，而当运行rosrun时，加sudo是会报错的。
因此不得不转化另一种思路就是用到什么就用下面这条命令提升什么权限。

sudo shmod 777 /dev/gpiomem
sudo shmod 777 /dev/mem

基本上就是上边这两个了，当我运行了上边这两条命令后，结果仍旧是报错，导致我不能完成在ros里控制pwm的目的，因此只能转换另一种方法了，如果有哪位大神知道怎么做，欢迎在评论区留言或者私聊我，感谢感谢。

Wiring Pi软件输出PWM

在这里先举一个简单的软件输出PWM的例子，led呼吸灯。

#include 
#include 
#include 
// 定义控制LED灯的GPIO引脚，使用GPIO0
#define PWM_PIN     0
int main(void)
{
    int bright ;
    printf("wiringPi-C Software PWM test programn") ;
    // 初始化
    wiringPiSetup();
    pinMode(PWM_PIN, OUTPUT);
    softPwmCreate(PWM_PIN, 0, 100);
    while(1) {
        for(bright = 0; bright < 100; bright++) {
            softPwmWrite(PWM_PIN, bright);
            delay(10);
        }
        for(bright = 100; bright >= 0; bright--) {
            softPwmWrite(PWM_PIN, bright);
            delay(10);
        }
    }
    return 0 ;
}

使用软件PWM接口需要包含头文件：#include
softPwmCreate (int pin, int value, int range)
用来创建软件控制PWM，能在任意GPIO口生成PWM波。
参数：
pin：要产生PWM波的GPIO引脚编号。
value：指定在PWM range 之间的任何一个初始值。
range：PWM的频率范围。设置range为100，则PWM频率为100Hz，value取值范围从0到100。这些值对产生不同占空比的PWM是有用的。
注：PWMfreq = 1 x 10^6 / (100 x range)，单位Hz，其中100是源码固定参数。

void softPwmWrite (int pin, int value)
参数：
pin：要控制的GPIO口，即在softPwmCreate函数中设置过的GPIO口。
value：占空比值，范围：0~range。

void softPwmStop (int pin)
参数：
pin：要关闭的GPIO口。

在这里如果有占空比和频率不太懂的可以参考这里或者这里
注意：PWM频率越高，所需要的CPU资源越多，特别注意需要寻求平衡。
然而在这里，问题又出现了，PWM是可以产生了，但是不够稳定，具体不稳定的形式是占空比设置一定的值后，会一直跳动，虽然说跳动的范围不大，但是对应到舵机上，反应的是舵机在抖，这种现象是极度不可忍的，因此不得不转向尝试另一种控制gpio的库 pigpio. 如果有人有这个问题的解决方案，欢迎评论区讨论。

另外Wiring Pi其他的控制pin口的用法可以参考这里

pigpio库手册这里也有：链接：https://pan.baidu.com/s/1ff-fwkx7640TgH6Mm2VmaQ
提取码：xqap

Pigpio方案

pigpio官网

Pigpio安装

安装方法有多种，采取其中一种就可以，不必重复安装：

直接安装

sudo apt-get update
sudo apt-get install pigpio python-pigpio python3-pigpio

make安装

可以采用make进行安装，有几种方法，请参考以下步骤：
请注意：如果你之前安装过，请先删除或者重命名任何现有的pigpio zip或tar文件。删除或重命名任何现有的PIGPIO或pigpio-master目录（记得要先保存该目录中的自己的文件）。
方法一：

rm pigpio.zip
sudo rm -rf PIGPIO
wget abyz.me.uk/rpi/pigpio/pigpio.zip
unzip pigpio.zip
cd PIGPIO
make
sudo make install

方法二：

rm pigpio.tar
sudo rm -rf PIGPIO
wget abyz.me.uk/rpi/pigpio/pigpio.tar
tar xf pigpio.tar
cd PIGPIO
make
sudo make install

方法三：

rm master.zip
sudo rm -rf pigpio-master
wget https://github.com/joan2937/pigpio/archive/master.zip
unzip master.zip
cd pigpio-master
make
sudo make install

用以上任何方法安装成功后，运行以下测试：

sudo ./x_pigpio // check C I/F
sudo pigpiod    // 开启守护进程
./x_pigpiod_if2 // check C      I/F to daemon
./x_pigpio.py   // check Python I/F to daemon
./x_pigs        // check pigs   I/F to daemon
./x_pipe        // check pipe   I/F to daemon

Pigpio输PWM

pigpio是一个由C语言编写的库函数，并提供Python接口。但是但是我大部分写的程序都是C++，因此我想找C的接口，可是在网上搜了很多资料，没找到。他们都是用的Python，因此我不得不产生了一个奇怪的想法，在C++里调用Python函数。。。最终还是成功了，不仅调用成功了而且PWM产生也很稳定。

先来说说Pigpio产生PWM 吧。可以参考这里

1.在运行编写好的代码之前，需要先执行以下命令

sudo pigpiod

这样做是为了开启一个线程用于该库的运行，如果不这么做，在运行代码时会提示错误。如果你运行完了程序，想要关闭该库，可以使用如下命令

sudo killall pigpiod

2.编写代码

import pigpio
import time
pi = pigpio.pi()          // 初始化
if not pi.connected:      // 检查是否连接成功 
   exit()
user_gpio = 26
pulsewidth = 1500         // 可以设置500至2500，这是电平为1的时间，单位是微秒
//为伺服电机产生PWM信号，设置频宽为1500 us，该信号频率默认为50Hz
pi.set_servo_pulsewidth(user_gpio, pulsewidth)
time.sleep(10)            // 延迟10秒
pi.wave_tx_stop() 
pi.wave_clear()
pi.stop()

这是一种比较初级的方法，不能改变频率(频率为50Hz,正好是控制舵机的频率)，占空比可以根据pulsewidth修改，对于更复杂，更实用的方法，参考[这里]，(https://blog.csdn.net/weixin_52157994/article/details/124054827?spm=1001.2014.3001.5502)
这个默认的频率已经满足我的要求了所以就直接不管更复杂的了，哈哈哈。

C++调用Python

可以参考这里，这里，这里有重要的接口及各函数解释

编写python文件：great_module.py

def great_function(a):
    print("hello python")
    return a + 1
   // 就这么多，不需要import 把这些内容写到great_module.py文件就可以了

编写.c文件：test.c

#include 
#include "track.h"

int great_function_from_python(int a) {
    int res;
    PyObject *pModule,*pFunc;
    PyObject *pArgs, *pValue;

    PyRun_SimpleString("import sys");
    PyRun_SimpleString("sys.path.append('./')");//若python文件在c++工程下
    PyRun_SimpleString("print(sys.path)");
    
    PyRun_SimpleString("import sys");
    pModule = PyImport_ImportModule("great_module");
    PyErr_Print();
    if (!pModule) {
        printf("Can not open python file!n");
        return -1;
    }

    
    pFunc = PyObject_GetAttrString(pModule, "great_function"); 
    
    
    pArgs = PyTuple_New(1);
    PyTuple_SET_ITEM(pArgs,0, PyLong_FromLong(a));
      
    
    pValue = PyObject_CallObject(pFunc, pArgs);
    
    res = PyLong_AsLong(pValue);
    return res;
}

int main(int argc, char *argv[]) {
    Py_Initialize();
    printf("%d",great_function_from_python(2));
    Py_Finalize();
}

输出结果：

hello python
3

CMakeList文件要更新一下，但也不用改太多，就是把对应的不用的动态库，源文件删掉。
CMakeLists.txt

cmake_minimum_required(VERSION 3.0.0)
project(c_python_test VERSION 0.1.0)

if(CMAKE_COMPILER_IS_GNUCC)
    message("COMPILER IS GNUCC")
        ADD_DEFINITIONS ( -std=c++11 )
endif(CMAKE_COMPILER_IS_GNUCC)

#SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -ggdb3")
SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")

# 添加头文件路径
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
include_directories(/home/dreamdeck/anaconda3/envs/track/include/python3.8) # 虚拟环境python头文件

# 添加链接库
LINK_DIRECTORIES(/home/dreamdeck/anaconda3/envs/track/lib)  #虚拟环境中python库的文件夹
LINK_LIBRARIES(python3.8)

# 添加要编译的可执行文件
add_executable(${PROJECT_NAME} test.c )

# 隐式链接库文件
# target_link_libraries(${PROJECT_NAME} python3.8)
#target_link_libraries(${PROJECT_NAME} track.cpython-38-x86_64-linux-gnu.so)

# 开启调试
SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -g")
message($(CMAKE_CXX_FLAGS))

到这里应该是可以了但是呢我在测试的时候一直返回 Can not open python file!
其实就是 PyImport_ImportModule 返回的为null，说白了就是找不到.py文件
其实可以根据 PyErr_Print(); 返回的错误信息进行修改，也就是在那个路径下找不到，解决方案可以参考这里,这里。

如果还不能解决，请把.py文件换一个位置再改个名字，哈哈哈，别问我怎么知道的
上边那个是参考的
具体到我的程序为：
PWM.py

def pwm_function(b):
    pi.set_servo_pulsewidth(26, b)
    return b
    
def Init_function(a):
    print("Init python")
    if not pi.connected:      # 检查是否连接成功 
        exit()
    user_gpio = 26
    pulsewidth = a         # 可以设置500至1500，这是电平为1的时间，单位是微秒
    pi.set_servo_pulsewidth(user_gpio, pulsewidth)
    return 

C++

int pwm_function_from_python(int a) {

    int res;
    PyObject *pModule,*pFunc;
    PyObject *pArgs, *pValue;
    PyObject *pName = NULL;
    
    
    if(b ==3){
    b=4;
    
    PyRun_SimpleString("import sys");
    PyRun_SimpleString("import os");

    
    PyRun_SimpleString("sys.path.append('/home/ubuntu/wsh/catkin_ws/src/learning_communication')");//若python文件在c++工程下
    PyRun_SimpleString("sys.path.append('./')");
    PyRun_SimpleString("print(sys.path)");
    PyErr_Print();
    
    PyRun_SimpleString("import sys");
    PyRun_SimpleString("import pigpio");
    PyRun_SimpleString("import time");

    pName = PyUnicode_FromString("PWM");
    pModule = PyImport_Import(pName);
    if (pModule == NULL) {
        PyErr_Print();
        printf("Can not open python file!n");
    }

    
    pFunc = PyObject_GetAttrString(pModule, "pwm_function");
    PyErr_Print();
    pFuncInit = PyObject_GetAttrString(pModule, "Init_function");
    PyErr_Print();
    
    pArgs = PyTuple_New(1);
    PyTuple_SET_ITEM(pArgs,0, PyLong_FromLong(a));
    
    
    pValue = PyObject_CallObject(pFuncInit, pArgs);
    PyErr_Print();
    res = PyLong_AsLong(pValue);
    }
    if(b != 3)
    {    
    pArgs = PyTuple_New(1);
    PyTuple_SET_ITEM(pArgs,0, PyLong_FromLong(a));
    pValue = PyObject_CallObject(pFuncInit, pArgs);
    PyErr_Print();
    res = PyLong_AsLong(pValue);} 

    return a;
}

这样确实能输出PWM 了，但是我不能在ROS执行的过程中随意更改占空比，如果更改会报错，具体报错原因是pi.connected连接的太多了，但是当我不连接直接调用pwm_function显示我pi没定义，就很尬。被逼无奈之下，我查了一下 PyRun_SimpleString() 这个函数，好像意思就是单行执行python代码，就好像在shell 环境一样，一句一句执行python代码。由于我这个python代码不复杂可以这样搞，因此我就改成了这样：

int pwm_function_from_python(int a, int leftright) {
    sprintf(str, "%d", a);
    if(leftright == 1)
    {stt = "pi.set_servo_pulsewidth(26,";}
    else
    {stt = "pi.set_servo_pulsewidth(16,";} 
    stt = stt + str + ")";
    strcpy(str,stt.c_str());
    
    if(b ==3){
    b=4;
    
    PyRun_SimpleString("import sys");
    PyRun_SimpleString("import os");

    
    PyRun_SimpleString("sys.path.append('/home/ubuntu/wsh/catkin_ws/src/learning_communication')");//若python文件在c++工程下
    PyRun_SimpleString("sys.path.append('./')");
    //PyRun_SimpleString("print(sys.path)");
    PyErr_Print();
    
    PyRun_SimpleString("import sys");
    PyRun_SimpleString("import pigpio");
    PyRun_SimpleString("import time");
    PyRun_SimpleString("pi = pigpio.pi() ");
    //PyRun_SimpleString("pi.set_servo_pulsewidth(26, 1400)");
    PyRun_SimpleString(str);
    if(b != 3)
    {PyRun_SimpleString(str);} 

    return a;
}

这样我重复执行 PyRun_SimpleString(“pi.set_servo_pulsewidth(26, 1400)”);这一行代码把1400改掉就可以了，不需要PWM.py文件了。

最后终于成功了，在ROS环境下随意使用服务与客户通信的方式随意更改PWM占空比。

参考

1.树莓派精确控制pwm输出，控制步进电机
2.[树莓派系列] 入门WiringPi库的PWM接口(C和Python)
3.树莓派 PWM输出
4.如何控制树莓派产生与读取pwm波——pigpio库函数使用指南:第一篇：基本介绍与安装
5.[树莓派] 使用pigpio库(3) 如何发送指定数量的脉冲信号
6.Ubuntu下C++调python
7.如何控制树莓派产生与读取pwm波——pigpio库函数使用指南:第三篇：PWM波形的产生