FeeTech 飞特舵机 在 ROS noetic 环境下的驱动

本文实现飞特总线舵机SM29-12在ROS noetic环境下的使用，教程同样适用于其他型号的飞特舵机。

准备工作 

搭建好舵机的驱动电路，包括 舵机、URT-1调试板以及电源，并将调试板通过USB线连接至电脑。

建议先在Windows环境下使用官方给的调试软件对舵机进行简单的测试，并确定舵机的ID号，以便在ROS中发送命令。

 前往官方gitee获取调试软件：

FTServo (ftservo) - Gitee.com

 下载SDK包

前往飞特gitee网页下载Linux版的SDK包。可以看到这里有很多其他平台的SDK包，包括STM32、Python等，还是非常丰富的。

飞特总线舵机SDK_FEETECH BUS Servo SDK : 飞特总线舵机接口库

解压后得到这些文件

 打开 说明.txt ，可以看到这里给出了SDK的结构。

这里笔者的舵机SM29-12属于SMSBL类，故在后续只需要SMSBL应用层程序，其他类的应用程序则不需要导入。

创建ROS功能包

笔者将其命名为 feetech_servo ，依赖包填入常用的 roscpp rospy std_msgs 即可。

catkin_create_pkg feetech_servo roscpp rospy std_msgs

导入前述头文件、源文件。

这里笔者的舵机属于SMSBL类，故在应用层只需导入该类的头文件、源文件。

这里注意，将所有的头文件放入 include/功能包名 文件夹下，源文件放入 src 文件夹下。

导入后，如下：

 

这里注意，需要将官方给出的源文件中的 #include 路径前加入功能包名。

驱动代码编写

在 src 下新建节点 cpp 文件，笔者将其命名为 servo_test 。

填入以下内容:

/*
舵机测试 节点

这里笔者的舵机ID号为5，
如有不同，请自行更改。
*/

#include "ros/ros.h"
// 导入应用层头文件，其中已包含底层头文件
#include "feetech_servo/SMSBL.h"

// 创建舵机对象
SMSBL sm;

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"servo_test");
    ros::NodeHandle nh;
    // 注意，使用时需在launch文件或是命令行中传入当前串口名称
    if(argc<2)
    {
        std::cout<< "argc error!"<<std::endl;
        return 0;
	}
	std::cout<< "serial:"<<argv[1]<<std::endl;
    // 注意，这里的波特率为115200，如不同，需自行修改
    if(!sm.begin(115200, argv[1]))
    {
        ROS_ERROR("驱动电机启动失败，请检查串口名称！");
        
        return 0;
    }
    // 读取位置、速度等的暂存变量
    int m_pos,m_spd,m_vol,m_crt,m_lod;

    while(ros::ok())
    {
        // 需将以下的 舵机ID 5 修改为自己的 舵机ID
        //舵机(ID5)以30步/秒，加速度A=10(10*100步/秒^2)，运行至P1=4095位置
		sm.WritePosEx(5, 4095, 30, 10);
        for(int i=0;i<10;i++)
        {
            m_pos=sm.ReadPos(5);
            // 10ms 延时
            ros::Duration(0.01).sleep();
            m_spd=sm.ReadSpeed(5);
            ros::Duration(0.01).sleep();
            m_vol=sm.ReadVoltage(5);
            ros::Duration(0.01).sleep();
            m_lod=sm.ReadLoad(5);
            ros::Duration(0.01).sleep();
            m_crt=sm.ReadCurrent(5);
            ros::Duration(0.01).sleep();

            // 向终端输出舵机信息
            ROS_INFO("POS: %d , Speed: %d",
            m_pos,m_spd);
            // 这里需要根据自己的舵机信息修改转换系数
            ROS_INFO("Voltage: %.2f V, Current: %.2f mA, Load: %.2f kg.N",
            (double)m_vol/10.0,(double)m_crt*6.5,(double)m_lod/1000.0*12.0);
            
            ros::Duration(0.5).sleep();
        }
        //舵机(ID5)以以30步/秒，加速度A=10(10*100步/秒^2)，运行至P0=0位置
		sm.WritePosEx(5, 0, 30, 10);
        for(int i=0;i<10;i++)
        {
            m_pos=sm.ReadPos(5);
            // 10ms 延时
            ros::Duration(0.01).sleep();
            m_spd=sm.ReadSpeed(5);
            ros::Duration(0.01).sleep();
            m_vol=sm.ReadVoltage(5);
            ros::Duration(0.01).sleep();
            m_lod=sm.ReadLoad(5);
            ros::Duration(0.01).sleep();
            m_crt=sm.ReadCurrent(5);
            ros::Duration(0.01).sleep();
            ROS_INFO("POS: %d , Speed: %d",
            m_pos,m_spd);
            // 这里需要根据自己的舵机信息修改转换系数
            ROS_INFO("Voltage: %.2f V, Current: %.2f mA, Load: %.2f kg.N",
            (double)m_vol/10.0,(double)m_crt*6.5,(double)m_lod/1000.0*12.0);
            ros::Duration(0.5).sleep();
        }
	}

	sm.end();

    return 0;
}

代码实现简单的舵机转动，以及位置、速度等的信息获取。注意，驱动板对应的串口信息需要在运行时传入。代码注释详细，可自行查看。

更多的函数可见于应用层头文件。

CMakeLists修改

这里主要注意，在添加可执行文件时需要列出SDK中的源文件。

具体如下：

cmake_minimum_required(VERSION 3.0.2)
project(feetech_servo)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES feetech_servo
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

include_directories(
  # 取消注释
include
  ${catkin_INCLUDE_DIRS}
)

# 添加可执行文件
# 这里注意，由于节点文件依赖其他源文件，需要一并列出
add_executable(servo_test src/servo_test.cpp src/SMSBL.cpp src/SCSerial.cpp src/SCS.cpp)

target_link_libraries(servo_test
  ${catkin_LIBRARIES}
)

编译，成功。

启动

首先给对应串口以访问权限。

sudo chmod 777 /dev/ttyUSB0

source 一下

source ./devel/setup.bash 

rosrun ，传入串口名称，执行程序。

rosrun feetech_servo servo_test /dev/ttyUSB0

 运行成功。

也可以编写launch文件，在launch文件中传入参数，更方便。

<launch>

    <node pkg="feetech_servo" type="servo_test" name="servo_test" args="/dev/ttyUSB0" output="screen" />

</launch>

 祝科研有进，前程似锦。