精华内容
下载资源
问答
  • 串口总线舵机之舵机命令

    千次阅读 2021-02-26 18:59:35
    驱动串口总线舵机 文章目录一、舵机指令包格式二、串口舵机连接1.硬件连接2.74HC126D三、关于ctypes四、串口舵机命令代码 从今天开始学习 幻尔科技总线舵机通信协议 一、舵机指令包格式 帧头: 连续收到两个 0x55 ...

    前言:
    关于一些前期配置,测试情况请观看下面这个博客
    驱动串口总线舵机


    从今天开始学习
    幻尔科技总线舵机通信协议
    关于这部分在SerialServoCmd文件里

    一、舵机指令包格式

    在这里插入图片描述

    帧头: 连续收到两个 0x55 ,表示有数据包到达。
    ID: 每个舵机都有一个 ID 号。ID 号范围 0~253,转换为十六进制 0x00~0xFD。广播 ID: ID 号 254(0xFE) 为广播 ID,若控制器发出的 ID 号为 254(0xFE),所有的舵机均接收指令,但都不返回应答信息,(读取舵机 ID 号除外,具体说明参见下面指令介绍)以防总线冲突。
    数据长度: 等于待发送的数据(包含本身一个字节)长度,即数据长度 Length加 3 等于这一包指令的长度,从帧头到校验和。
    指令: 控制舵机的各种指令,如位置、速度控制等。
    参数: 除指令外需要补充的控制信息。
    校验和: 校验和 Checksum,计算方法如下:
    Checksum = ~ (ID + Length + Cmd+ Prm1 + … Prm N)若括号内的计算和超出 255,则取最低的一个字节,“~”表示取反。

    说明:
    数据长度为 数据长度+指令+ 参数+校验和 参数个数 = 数据长度-3

    二、串口舵机连接

    1.硬件连接

    在这里插入图片描述

    2.74HC126D

    74HC126D介绍:
    功能描述:
    在这里插入图片描述
    可以看出,当OE输出高电平时 输入是高电平那么输出就是高电平,输入是低电平输出就是低电平。

    OE为低电平时不管输入状态是什么,输出都是高阻抗关断状态(抽象理解为悬空)

    高阻输出一般是指数字电路输出时不为高电平或低电平,而是相当于断开的一种状态,输出点的电位由后面的电路决定。

    这个芯片的作用就是,当需要写入的时候,拉低TX_CON,这样,串口TX发送什么,输出就是什么。拉高RX_CON,这样,串口接收RX就相当于悬空,什么也不干。接收数据也是如此.

    三、关于ctypes

    关于ctypes的介绍
    ctypes

    四、串口舵机命令代码

    # 串口舵机的命令
    #!/usr/bin/python3
    # encoding: utf-8
    import serial # 导入串口库
    import pigpio # 导入pigpio库  由c语言编写的库函数 并提供python接口
    import time # 导入时间库
    import ctypes # ctypes是Python的一个外部库,提供和C语言兼容的数据类型,可以很方便地调用C DLL中的函数。
    
    LOBOT_SERVO_FRAME_HEADER         = 0x55  # 机架头
    LOBOT_SERVO_MOVE_TIME_WRITE      = 1     # 移动时间写入
    LOBOT_SERVO_MOVE_TIME_READ       = 2     # 移动时间读
    LOBOT_SERVO_MOVE_TIME_WAIT_WRITE = 7     # 移动时间等待写
    LOBOT_SERVO_MOVE_TIME_WAIT_READ  = 8     # 移动时间等待读
    LOBOT_SERVO_MOVE_START           = 11    # 移动开始
    LOBOT_SERVO_MOVE_STOP            = 12    # 移动停止
    LOBOT_SERVO_ID_WRITE             = 13    # 舵机ID写
    LOBOT_SERVO_ID_READ              = 14    # 舵机ID读
    LOBOT_SERVO_ANGLE_OFFSET_ADJUST  = 17    # 角度偏移调整
    LOBOT_SERVO_ANGLE_OFFSET_WRITE   = 18    # 角度偏移写
    LOBOT_SERVO_ANGLE_OFFSET_READ    = 19    # 角度偏移读
    LOBOT_SERVO_ANGLE_LIMIT_WRITE    = 20    # 角度限制写
    LOBOT_SERVO_ANGLE_LIMIT_READ     = 21    # 角度限制读
    LOBOT_SERVO_VIN_LIMIT_WRITE      = 22    # VIN限制写
    LOBOT_SERVO_VIN_LIMIT_READ       = 23    # VIN限制读
    LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE = 24    # TEMP最大限度写
    LOBOT_SERVO_TEMP_MAX_LIMIT_READ  = 25    # TEMP最大限度读
    LOBOT_SERVO_TEMP_READ            = 26    # TEMP读
    LOBOT_SERVO_VIN_READ             = 27    # VIN读
    LOBOT_SERVO_POS_READ             = 28    # POS读
    LOBOT_SERVO_OR_MOTOR_MODE_WRITE  = 29    # 模式写
    LOBOT_SERVO_OR_MOTOR_MODE_READ   = 30    # 模式读
    LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE = 31    # 加载或卸载写
    LOBOT_SERVO_LOAD_OR_UNLOAD_READ  = 32    # 加载或卸载读
    LOBOT_SERVO_LED_CTRL_WRITE       = 33    # LED控制写
    LOBOT_SERVO_LED_CTRL_READ        = 34    # LED控制读
    LOBOT_SERVO_LED_ERROR_WRITE      = 35    # LED错误写
    LOBOT_SERVO_LED_ERROR_READ       = 36    # LED错误读
    
    pi = pigpio.pi()  # 初始化pigpio库,创建一个实例
    # ser=serial.Serial("/dev/ttyUSB0",9600,timeout=0.5) #使用USB连接串行口
    # ser=serial.Serial("/dev/ttyAMA0",9600,timeout=0.5) #使用树莓派的GPIO口连接串行口
    serialHandle = serial.Serial("/dev/ttyAMA0", 115200)  # 初始化串口, 波特率为115200
    
    
    # 使用pigpio配置驱动串口的引脚模式为输出
    def portInit():  # 配置用到的IO口
     # 说明  pigpio使用的是BCM编码  
        # RX_CON和TX_CON是使能信号  来决定TX还是RX输出servo signal
        pi.set_mode(17, pigpio.OUTPUT)  # 配置RX_CON 即 GPIO17 为输出
        pi.write(17, 0)# GPIO 17 低电平
        pi.set_mode(27, pigpio.OUTPUT)  # 配置TX_CON 即 GPIO27 为输出
        pi.write(27, 1) # GPIO 27 高电平
    
    
    portInit() # 执行端口初始化
    
    # 配置为串口写模式
    def portWrite():  # 配置单线串口为输出
        pi.write(27, 1)  # 拉高TX_CON 即 GPIO27  串口发送的是什么输出就是什么
        pi.write(17, 0)  # 拉低RX_CON 即 GPIO17  相当于悬空接受引脚,什么都不干
    
    # 配置为串口读模式
    def portRead():  # 配置单线串口为输入
        pi.write(17, 1)  # 拉高RX_CON 即 GPIO17
        pi.write(27, 0)  # 拉低TX_CON 即 GPIO27 悬空
    
    # 复位,重新打开串口
    def portRest(): # 端口复位
        time.sleep(0.1)   # 延迟100us
        serialHandle.close()# 关闭串口
        pi.write(17, 1)
        pi.write(27, 1)
        serialHandle.open() # 打开串口
        time.sleep(0.1)
    
    # 校验和 = ~(ID + Length+ Cmd + pr1+ .. + prn) 若超出255,则取最低的一个字节
    def checksum(buf):
        # 计算校验和
        sum = 0x00
        for b in buf:  # 求和
            sum += b # 累加
        sum = sum - 0x55 - 0x55  # 去掉命令开头的两个 0x55
        sum = ~sum  # 取反
        return sum & 0xff  # 取最低的一个字节
    
    # 串口舵机写命令
    # 指令包格式:0x55,0x55 ID号 数据长度,指令,参数1...参数n,校验和
    # 数据长度等于待发送的数据(包含本身1个字节)
    def serial_serro_wirte_cmd(id=None, w_cmd=None, dat1=None, dat2=None):
        portWrite()  # 端口写
            # bytearray() 方法返回一个新字节数组。这个数组里的元素是可变的,并且每个元素的值范围: 0 <= x < 256
        '''
        如果 source 为整数,则返回一个长度为 source 的初始化数组;
        如果 source 为字符串,则按照指定的 encoding 将字符串转换为字节序列;
        如果 source 为可迭代类型,则元素必须为[0 ,255] 中的整数;
        如果 source 为与 buffer 接口一致的对象,则此对象也可以被用于初始化 bytearray。
        如果没有输入任何参数,默认就是初始化数组为0个元素。
        '''
        # b'\x55\x55')    [0x55,0x55]
        buf = bytearray(b'\x55\x55')  # 帧头   buf = [0x55,0x55]
        buf.append(id) # buf = [0x55,0x55,id]
        # 指令长度
        if dat1 is None and dat2 is None:# dat1和dat2都为空
            buf.append(3) # buf = [0x55,0x55,id,3]  3个表示指令长度 指令 检验和
        elif dat1 is not None and dat2 is None: # dat1不为空dat2为空
            buf.append(4) # buf = [0x55,0x55,id,4] 4个表示指令长度,指令 dat1(低8位),校验和
        elif dat1 is not None and dat2 is not None: #dat1 和dat2都不为空
            buf.append(7)  # buf = [0x55,0x55,id,7] 7个表示指令长度,指令,dat1(高8位,低8位) dat2(高8位,低8位),校验和
    
        buf.append(w_cmd) # 把指令也添加到列表  # buf = [0x55,0x55,id,x,w_cmd]
        # 写数据
        if dat1 is None and dat2 is None: # dat1和dat2都为空
            pass # buf = [0x55,0x55,id,x,w_cmd]
        elif dat1 is not None and dat2 is None: # dat1不为空dat2为空
         # (dat1 & 0xff) 取最低位
            buf.append(dat1 & 0xff)  # buf = [0x55,0x55,id,x,w_cmd,dat1]
        elif dat1 is not None and dat2 is not None: # dat1 和 dat2 都不为空
            buf.extend([(0xff & dat1), (0xff & (dat1 >> 8))])  # 分低8位 高8位 放入缓存
            buf.extend([(0xff & dat2), (0xff & (dat2 >> 8))])  # 分低8位 高8位 放入缓存
        # 可能是buf = [0x55,0x55,id,x,w_cmd,dat1,dat2]
              
        # 校验和
        buf.append(checksum(buf)) # 到这数据有三种情况
        serialHandle.write(buf)  # 发送给串口
        
    # 串口舵机读命令 先发送读命令 再接收,不单独使用
    def serial_servo_read_cmd(id=None, r_cmd=None):
        portWrite() # 端口写
        buf = bytearray(b'\x55\x55')  # 帧头
        buf.append(id)# 添加舵机ID到列表
        buf.append(3)  # 指令长度
        buf.append(r_cmd)  # 指令
        buf.append(checksum(buf))  # 校验和
        serialHandle.write(buf)  # 发送
        time.sleep(0.00034) # 延迟 3.4us
    
    # 获取指定读取命令的数据
    def serial_servo_get_rmsg(cmd):
        serialHandle.flushInput()  # 清空接收缓存
        portRead()  # 将单线串口配置为输入
        time.sleep(0.005)  # 稍作延时,等待接收完毕
        count = serialHandle.inWaiting()    # 获取接收缓存中的字节数
        if count != 0:  # 如果接收到的数据不空
            recv_data = serialHandle.read(count) # 读取count字节个数据
            try:
            # recv_data[2] : id   recv_data[3]: 数据长度 
                if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == cmd: # 帧头正确  命令符合 匹配
                    dat_len = recv_data[3]  # 数据长度
                    serialHandle.flushInput()  # 清空接收缓存
                    # 数据长度为 数据长度+指令+ 参数+校验和  参数个数 = 数据长度-3
                    if dat_len == 4: # dat1为一个8位的参数dat2为空
                        # print ctypes.c_int8(ord(recv_data[5])).value    # 转换成有符号整型
                        return recv_data[5] # 返回这个8位的参数
                    elif dat_len == 5:  # dat1为一个16位的参数dat2为空
                        pos = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8))) # 一个16位的数据
                        return ctypes.c_int16(pos).value
                    elif dat_len == 7: # dat1位16位的参数,dat2为16位的参数
                        pos1 = 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8)))
                        pos2 = 0xffff & (recv_data[7] | (0xff00 & (recv_data[8] << 8)))
                        return ctypes.c_int16(pos1).value, ctypes.c_int16(pos2).value
                else:  # 数据不正确,不符合接收数据的格式
                    return None
            except BaseException as e:
                print(e) # 打印异常
        else:  # 接收数据为空
            serialHandle.flushInput()  # 清空接收缓存
            return None
    
    
    展开全文
  • 乐幻索尔总线舵机通信协议
  • 寻黑线小车,5个红外传感器,小车动力用的是串口总线舵机,所以速度控制是通过发送串口指令的,红外通过外部中断触发
  • 总线舵机控制板STM32F103RCT6,四路总线引出,单电源供电,防反接保护。
  • 在我们做机器人项目的时候,不可避免的要和总线舵机打交道,今天就介绍怎么去测试总线舵机。 前言 驱动舵机之前需要先去安装pigpio库和配置串口 一、安装pigpio库 二、配置串口 一、硬件连接 二、编写程序 #!/usr/...


    在我们做机器人项目的时候,不可避免的要和总线舵机打交道,今天就介绍怎么去测试总线舵机。

    前言

    驱动舵机之前需要先去安装pigpio库和配置串口
    一、安装pigpio库
    二、配置串口

    一、硬件连接

    在这里插入图片描述

    二、编写程序

    #!/usr/bin/python3
    
    import serial
    import pigpio
    import time
    
    pi = pigpio.pi()  #初始化 pigpio库
    serialHandle = serial.Serial("/dev/ttyAMA0", 115200)  #初始化串口, 波特率为115200
    
    
    ##
    ##命令发送
    ##
    def servoWriteCmd(id, cmd, par1 = None, par2 = None):
        buf = bytearray(b'\x55\x55')
        try:
            len = 3   #若命令是没有参数的话数据长度就是3
            buf1 = bytearray(b'')
    
    	## 对参数进行处理
            if par1 is not None:
                len += 2  #数据长度加2
                buf1.extend([(0xff & par1), (0xff & (par1 >> 8))])  #分低8位 高8位 放入缓存
            if par2 is not None:
                len += 2
                buf1.extend([(0xff & par2), (0xff & (par2 >> 8))])  #分低8位 高8位 放入缓存
            buf.extend([(0xff & id), (0xff & len), (0xff & cmd)])
            buf.extend(buf1) #追加参数
    
    	##计算校验和
            sum = 0x00
            for b in buf:  #求和
                sum += b
            sum = sum - 0x55 - 0x55  #去掉命令开头的两个 0x55
            sum = ~sum  #取反
            buf.append(0xff & sum)  #取低8位追加进缓存
            serialHandle.write(buf) #发送
        except Exception as e:
            print(e)
    
    def portInit(): #配置用到的IO口
        pi.set_mode(17, pigpio.OUTPUT)  #配置RX_CON 即 GPIO17 为输出
        pi.write(17, 0)
        pi.set_mode(27, pigpio.OUTPUT)  #配置TX_CON 即 GPIO27 为输出
        pi.write(27, 1)
    
    def portWrite():  #配置单线串口为输出
        pi.write(27, 1)  #拉高TX_CON 即 GPIO27
        pi.write(17, 0)  #拉低RX_CON 即 GPIO17
        
    
    def portRead():   #配置单线串口为输入
        pi.write(27, 0) #拉低TX_CON 即 GPIO27
        pi.write(17, 1) #拉高RX_CON 即 GPIO17
    
    
    portInit()
    while True:
        try:
            portWrite() #将单线串口配置为输出
            servoWriteCmd(1,1,0,1000) #发送命令 参数1 舵机id=1, 参数2 命令 = 1, 参数3 位置 = 0, 参数4 时间 = 1000ms
            time.sleep(1.1)
            servoWriteCmd(1,1,1000,2000)
            time.sleep(2.1)
        except Exception as e:
            print(e)
            break
    
    

    需要修改为对应的舵机的ID,这里使用的是1,舵机出厂默认是1

    三、测试

    直接在树莓派运行这个程序,发现舵机会转动
    在这里插入图片描述
    测试成功。

    展开全文
  • ros总线舵机机械臂串口协议源码

    千次阅读 2017-11-20 16:56:21
    总线舵机机械臂moveit规划效果 现场 前文有普通模拟舵机diy的舵机手臂. 型号996r普通舵机用在手臂上可以说没有什么精度可言扭力也不行,ros试验无法使用. 所以diy手臂时选择舵机还是非常关键的...


    总线舵机机械臂moveit规划效果


    现场



    前文有普通模拟舵机diy的舵机手臂.

    型号996r普通舵机用在手臂上可以说没有什么精度可言扭力也不行,ros试验无法使用.


    所以diy手臂时选择舵机还是非常关键的.

    某宝上有zx361s舵机做的手臂看起来还行.

    zx361s舵机是总线舵机.

    支持位置回读.

    串口通信协议

    查看文档可以用各种简单字符串命令控制舵机

    经测试其扭力,精度和线性度还可以

    支持回读可以将其位置实时发布出去供moveit 使用.

    以下代码实现了与手臂舵机通信的功能


    #include <ros/ros.h>
    #include <serial/serial.h>
    //ROS已经内置了的串口包
    #include <std_msgs/String.h>
    #include <std_msgs/Empty.h>
    #include "zzz_arm_2_control_driver/joint_msg.h"
    #include <boost/asio.hpp>                  //包含boost库函数
    
    #include <stdlib.h>
    #include<time.h>
    #include <sensor_msgs/JointState.h>
    #include <tf/transform_broadcaster.h>
    
    using namespace boost::asio;           //定义一个命名空间,用于后面的读写操作
    using namespace std;
    serial::Serial ser; //声明串口对象
    //io_service Object
    io_service m_ios;
    
    //Serial port Object
    serial_port *pSerialPort;
    
    //For save com name
    //any_type m_port;
    
    //Serial_port function exception
    boost::system::error_code ec;
    
    
    std::string serial_port_name;
    int serial_baudrate = 115200;
    unsigned char AA=1;
    unsigned char aa;
    //回调函数
    void write_callback(const zzz_arm_2_control_driver::joint_msg::ConstPtr& msg)
    {
        //ROS_INFO_STREAM("Writing to serial port" <<msg->id<<msg->r);
        unsigned char mid=(char) msg->id;
        double r=msg->r;
        double pi=3.1415926535;
        double per;
        if(mid==1){
          //-90~90
          per=2.0*r/pi;
        }else{
          //-135~135
          per=4.0/3.0*r/pi;
        }
    
        int posi=1500+int(per*1000.0);
        posi=posi<500?500:posi;
        posi=posi>2500?2500:posi;
        string sendData="#00";
        string c2str(1,'0'+mid);
        sendData+=c2str;
        sendData+="P";
        //string si=itoa(posi);
        ostringstream os;
        os << posi;
        string si = os.str();
        for (char i=0;i<4-si.length();i++){
          sendData+="0";
        }
        sendData+=si;
        sendData+="T0100!";
        //ROS_INFO("-------------%s",sendData.c_str());
        //#000P1500T1000!
        //{#000P1500T1000!#001P1500T1000!}
        //发送串口数据
        try
        {
          size_t len = write( *pSerialPort, buffer( sendData,sendData.length() ), ec );
          ROS_INFO("positon command send: %s",sendData.c_str());
        }catch (boost::system::system_error e){
          ROS_ERROR_STREAM("serail write err ");
    
        }
    }
    unsigned char buf[1];                      //定义字符串长度
    unsigned char data[255];//
    unsigned int dn=0;
    unsigned int i=0,j=0;
    unsigned char isWaittingPosi=0;
    unsigned char waitId=0;
    unsigned char waittingId=0;
    unsigned char maxId=4;//input a wrong maxid can run test code when time is out.
    unsigned char waitDida=0;
    unsigned char waitDidaMax=50;//wait dida
    int main (int argc, char** argv)
    {
    
        //初始化节点
        ros::init(argc, argv, "arm2_jointserial");
        //声明节点句柄
        ros::NodeHandle nh;
    
        //订阅主题,并配置回调函数
        ros::Subscriber write_sub = nh.subscribe("/arm_motors", 1000, write_callback);
        //发布主题
        ros::Publisher joint_motor_pub = nh.advertise<zzz_arm_2_control_driver::joint_msg>("/arm_motors", 1000);
        zzz_arm_2_control_driver::joint_msg joint_motor_msg;
        //ros::Publisher read_pub = nh.advertise<std_msgs::String>("read", 1000);
        ros::Publisher joint_pub = nh.advertise<sensor_msgs::JointState>("/move_group/fake_controller_joint_states", 1);
    
        sensor_msgs::JointState joint_state_msg;
        /*
          <param name="shoulder_zhuan_joint" value="0" />
          <param name="upper_arm_joint" value="1" />
          <param name="fore_arm_joint" value="2" />
          <param name="hand_wan_joint" value="3" />
          <param name="hand_zhuan_joint" value="4" />
    */
        string jointnames[5] = {"shoulder_zhuan_joint",
                               "upper_arm_joint",
                               "fore_arm_joint",
                               "hand_wan_joint",
                               "hand_zhuan_joint"};
    
        ros::NodeHandle nh_private("~");
        nh_private.param<std::string>("serial_port", serial_port_name, "/dev/ttyUSB0");
        nh_private.param<int>("serial_baudrate", serial_baudrate, 115200);
    
    
        pSerialPort = new serial_port( m_ios );
        if ( pSerialPort ){
            //init_port( port_name, 8 );
            //Open Serial port object
            pSerialPort->open( serial_port_name, ec );
            //Set port argument
            pSerialPort->set_option( serial_port::baud_rate( serial_baudrate ), ec );
            pSerialPort->set_option( serial_port::flow_control( serial_port::flow_control::none ), ec );
            pSerialPort->set_option( serial_port::parity( serial_port::parity::none ), ec );
            pSerialPort->set_option( serial_port::stop_bits( serial_port::stop_bits::one ), ec);
            pSerialPort->set_option( serial_port::character_size( 8 ), ec);
            m_ios.run();
        }
    
        ROS_INFO("-------------zzz joint serail is running .");
    
        ros::Rate zzzrat(100.0);
    
    
        while(nh.ok()){
          //check time is out.
          if(waitDida>waitDidaMax){
            waitDida=0;
            isWaittingPosi=0;
            ROS_INFO("error: time is out when conect motor %d.",waittingId );
    
            //test random position
            if(0){
              srand(time(0));
              joint_motor_msg.id=3;
              joint_motor_msg.r=3.14* (((float)rand()/RAND_MAX)-0.5)*2.0;
              joint_motor_pub.publish(joint_motor_msg);
            }
          }
          //send new request
          if(isWaittingPosi==0){
            //#000PRAD!
            //send position request ,result is #000P1500!
    
           /* stringstream ss;
            int n = int (waitId);
            string str;
            ss<<n;
            ss>>str;
            */
            string sendData="#00";
            string c2str(1,'0'+waitId);//<10
            sendData+=c2str;
            sendData+="PRAD!";
            try
            {
              size_t len = write( *pSerialPort, buffer( sendData,sendData.length() ), ec );
              ROS_INFO("positon request send: %s",sendData.c_str());
              isWaittingPosi=1;
              waittingId=waitId;
              //
              waitId+=1;
              waitId=(waitId>maxId)?0:waitId;
            }catch (boost::system::system_error e){
                ROS_ERROR_STREAM("serail write err ");
    
            }
          }
          //read
          while(1){
            try
            {
              read (*pSerialPort,buffer(buf));
              data[dn]=buf[0];
              dn++;
              if(dn>=64){break;}
              //ROS_INFO("%d", buf[0]);//打印接受到的字符串
            }catch (boost::system::system_error e){
              //ROS_ERROR_STREAM("read err ");
              break;
            }
          }
          //ROS_INFO("read");
          //parse package//{'#'...'!'}
          vector <string> result;
          string str="";
          unsigned char findHead=0;
          unsigned char findTail=0;
          unsigned char ti=0;
          for(i=0;i<dn;i++){
            if((data[i])=='#'){
              findHead=1;
              findTail=0;
              str="";
              for(ti=i;ti<dn;ti++){
                str+=data[ti];
                if((data[ti])=='!'){
                  findTail=1;
                  break;
                }
              }
              if(findTail){
                i=ti;
                result.push_back(str);
                //here save result
    
              }else{
                break;
              }
            }else{
              //ignore
            }
          }
          if(i>0){
            ROS_INFO("read buffer lenght=%d",i);
          }
          //buffer move to 0
          for(j=0;i<dn;i++,j++){
            data[j]=data[i];
          }
          dn=j;
          //parase strings
          int count = result.size();
          for (int ri = 0; ri < count;ri++)
          {
            cout << result[ri] << endl;
            string s1,s2,s3;
            s1=result[ri];
            //#000P1500!
            //need check package data here!!!!
            size_t iPos = s1.find("P");
            if(s1.length()==10&&iPos==4){
              s2 = s1.substr(iPos-3, 3);
              s3 = s1.substr(iPos+1, 4);
              int id=atoi(s2.c_str());
              int posi=atoi(s3.c_str());
              ROS_INFO("get position result.  id= %d ,posi=%d",id,posi );
    
              joint_state_msg.header.stamp = ros::Time::now();
              joint_state_msg.name.resize(1);
              joint_state_msg.position.resize(1);
              joint_state_msg.name[0] =jointnames[id];
              double per=double (posi-1500)/1000.0;
              double pi=3.1415926535;
              double r;
              if(id==1){
                //-90~90
                r=per*pi/2.0;
              }else{
                //-135~135
                r=per*pi/4.0*3.0;
              }
              joint_state_msg.position[0] = r ;
    
              joint_pub.publish(joint_state_msg);
              ros::spinOnce();
              //publishmsg()
              //.....to do something
            }else{
              ROS_INFO("warnning. package data cant be parsed :%s",s1.c_str() );
            }
    
            isWaittingPosi=0;
            waitDida=0;
    
          }
    
          zzzrat.sleep();
          //ROS_INFO("loop---------------");
          waitDida+=1;
        }
    
    }
    
    




    前文:

    ros之真实驱动diy6自由度机械臂

    ros之真实驱动diy6自由度机械臂(moveit中controller源码)
    展开全文
  • 我们如何通过外部去控制树莓派串口总线舵机和PWM舵机。 网络无疑是一个很好的选择。 这一部分是在lsc.py文件中 关于socketserver部分参考 socketserver 文章目录调试工具1.网络调试助手2.树莓派一、控制命令1.设置...

    我们如何通过外部去控制树莓派串口总线舵机和PWM舵机。
    网络无疑是一个很好的选择。
    这一部分是在lsc.py文件中
    关于socketserver部分参考
    socketserver

    调试工具

    1.网络调试助手

    在这里插入图片描述

    2.树莓派

    在这里插入图片描述

    一、控制命令

    1.设置PWM舵机位置

    设置PWM舵机的位置,一次设置一个舵机
    指令格式
    55 55 08 3 xx xx xx id posl posh

    id = 6 或者是等于7
    7是控制摄像头上下的舵机
    6是控制摄像头左右的舵机
    posl :位置的低位
    posh:位置的高位

    示例

    55 55 08 3 0 0 0 7 0 0
    

    这是设置舵机ID7设置位置为3000-1200 = 1800
    说明:
    位置小于1200,就是1200,
    位置大于1900,就是1900

    测试
    在这里插入图片描述
    这里的0是传进去的参数,实际的值没有输出,懒的改了

    2.运行动作组

    发送这个指令可以运行一组动作
    指令格式
    55 55 05 06 action_num action_timel action_timeh

    action_num :运行的动作组的名字
    action_timel :运行的次数的低位
    action_timeh:运行的次数的高位

    示例

    55 55 05 06 0 0 2
    

    运行立正动作 2次

    测试
    在这里插入图片描述

    3.复位两个PWM舵机

    运行这个指令,可以使两个PWM舵机复位到中位的位置
    指令格式
    55 55 02 03

    没有参数

    示例

    55 55 02 03
    

    将两个PWM舵机复位到1500的位置

    测试
    在这里插入图片描述

    4.读出舵机的电压值

    运行这个指令可以读出舵机的电压值
    指令格式
    55 55 03 0f id

    id:是舵机的ID

    示例

    55 55 03 0f 6
    

    这里读取的是ID号为6的舵机的电压值

    测试
    树莓派运行结果:
    在这里插入图片描述
    网络助手运行结果:
    在这里插入图片描述

    二、代码

    # 网络控制
    #!/usr/bin/python3
    # -*- coding: UTF-8 -*-
    
    import socketserver # 提供了服务器中心类,可以简化网络服务器的开发
    import PWMServo  # 导入PWM舵机二次封装
    import Serial_Servo_Running as SSR # 动作组调用
    import threading # 线程
    import get_data # 从文件中获取PID的值
    import math # 导入数学库
    from config_serial_servo  import  serial_servo_read_vin #导入读取舵机的电压
    import time # 导入时间模块
    import os # 导入系统模块
    
    DEBUG = False # 开启调试模式
    
    client_socket = []  # 创建一个客户端列表 
    
    # 开启动作组线程,动作一发生变化,就会去执行动作
    SSR.start_action_thread() # 开启动作组线程
    
    # TCPServer是接收到请求后执行handle方法,如果前一个的handle没有结束,那么其他的请求将不会受理,新的客户端也无法加入。
    # 而ThreadingTCPServer和ForkingTCPServer则允许前一连接的handle未结束也可受理新的请求和连接新的客户端,
    # 区别在于前者用建立新线程的方法运行handle,后者用新进程的方法运行handle
    # 机器人的服务类 负责绑定,监听,运行
    class LobotServer(socketserver.ThreadingTCPServer):  # 继承ThreadingTCPServer
        allow_reuse_address = True  # 允许地址重用
        
    # 机器人的请求处理类  负责处理用户所发送的数据  定义一个类,继承socketserver.BaseRequestHandler
    class LobotServerHandler(socketserver.BaseRequestHandler): 
        global client_socket # 全局变量 客户端列表
        ip = "" # ip地址
        port = None # 端口号
        # setup在handle()之前被调用,主要的作用就是执行请求之前的初始化相关的工作。
        def setup(self):
            # strip()方法用于移除字符串头尾指定的字符(默认为空格或换行符)或字符序列
            # client_address获取的是一个元组 包含ip地址和端口
            self.ip = self.client_address[0].strip() # 获取客户端1的IP地址
            self.port = self.client_address[1]  # 获取端口号
            print("connected\tIP:"+self.ip+"\tPort:"+str(self.port))
            client_socket.append(self.request)  # 将此连接加入客户端列表
            #self.request.settimeout(6)  # 超时时间为6秒
        # 这个方法必须有,每一个客户端链接之后都会执行这个函数
        # 工作就是做那些所有与处理请求相关的工作
        def handle(self):
            global action_num, action_times, inf_flag
            conn = self.request  # 获取连接的客户端的对象
            recv_data = b''  # 接收数据都是字节类型的
            Flag = True  # 循环标志  循环
            stop = False # 结束标志  
            t2 = 0
            while Flag:    # 循环
                try:
                    buf = conn.recv(128)  # 接收128个字节数据 可以小于这个长度
                    # Python3的字符串的编码语言用的是unicode编码,由于Python的字符串类型是str,在内存中以Unicode表示,
                    # 一个字符对应若干字节,如果要在网络上传输,或保存在磁盘上就需要把str变成以字节为单位的bytes
                    if buf == b'': # 空字符串  没有接收到数据
                        Flag = False # 退出循环
                    else: # 接收到数据
                        # 打印出数据
                     
                        print('收到的数据是:',(buf))
                            
                        recv_data = recv_data + buf # 将字符串转化为字节类型
                        if len(recv_data) <= 13:     # 接收数据长度小于等于13
                            print('收到%d个字节' %(len(recv_data)))
                            # 解决断包问题,接收到完整命令后才发送到串口,防止出错
                            while True: # 死循环
                                try:
                                    index = recv_data.index(b'\x55\x55')  # 搜索数据中的0x55 0x55的下标
                                    print('index is :',index)
                                    # 加3 index返回的是第一个0x55的位置,两个0x55 2个字节 加3表示后面还有数据
                                    if len(recv_data) >= index+3:  # 缓存中的数据长度是否足够
                                    # 判断下面接受的指令
                                        recv_data = recv_data[index:] # 截取完整的指令 包含帧头
                                        # recv_data[2]是去掉帧头的第一个数据 也就是cmd数据的长度 -2
                                        # 下面是判断数据是否正确 cmd小于一帧数据
                                        if recv_data[2] + 2 <= len(recv_data):  # 缓存中的数据长度是否足够
                                            cmd = recv_data[0:(recv_data[2]+2)]    # 取出命令,包含帧头
                                            #print(cmd)
                                            recv_data = recv_data[(recv_data[2]+3):]  # 去除已经取出的命令
                                            # 0x55 0x55 08 03 xx xx xx id posl posh  # 设置PWM舵机的位置 id=6/7
                                            # 55 55 03 0F id  # 读舵机的电压值
                                            # 55 55 05 06 action_num action_timesl action_timesh # 运行动作组
                                            # 55 55 02 03   # 复位两个PWM舵机
                                            if cmd[0] and cmd[1] is 0x55:
                                                if cmd[2] == 0x08 and cmd[3] == 0x03:  # 数据长度和控制单舵机命令
                                                   
                                                    id = cmd[7]
                                                    pos = 0xffff & cmd[8] | 0xff00 & (cmd[9] << 8)
                                                    # print('id:', cmd[7], 'pos:', pos)
                                                    print('设置舵机%d的位置为%d:' %(id,pos))
                                                    if id == 7:
                                                        if 1900 < pos:
                                                            pos = 1900
                                                        elif pos < 1200:
                                                            pos = 1200
                                                        PWMServo.setServo(1, 3000 - pos, 20)
                                                    elif id == 6:
                                                        PWMServo.setServo(2, 3000 - pos, 20)
                                                    else:
                                                        pass
                                                # 55 55 05 06 action_num action_timesl action_timesh
                                                elif cmd[2] == 0x05 and cmd[3] == 0x06: # 数据长度和控制舵机动作组命令
                                                    action_num = cmd[4]
                                                    action_times = 0xffff & cmd[5] | 0xff00 & cmd[6] << 8
                                                    print('运行动作组')
                                                    # 站立动作 
                                                    if action_num == 0:
                                                        SSR.stop_action_group()
                                                        # 缓慢站立 运行一次
                                                        SSR.change_action_value('stand_slow', 1)
                                                        try:
                                                            # 读取pid文件
                                                            data = get_data.read_data()                                          
                                                            if data[0] != 'K':
                                                                os.system('sudo kill -9 ' + str(data[0]))
                                                                PWMServo.setServo(1, 1500, 300)
                                                                PWMServo.setServo(2, 1500, 300)                                                        
                                                                get_data.write_data("K", "0")
                                                        except BaseException as e:
                                                            print(e)
                                                    # 无限次   
                                                    elif action_times == 0:  # 无限次                                                    
                                                        print('action', action_num, 'times', action_times)
                                                        SSR.change_action_value(str(action_num), action_times)
                                                        stop = True
                                                    else:
                                                        print('action', action_num, 'times', action_times)
                                                        if stop:                                                       
                                                            SSR.stop_action_group()
                                                            # 缓慢站立 执行一次
                                                            SSR.change_action_value('stand_slow', 1)
                                                            stop = False
                                                        else:
                                                            print('action', action_num, 'times', action_times)
                                                            SSR.change_action_value(str(action_num), action_times)
                                                
                                                # 55 55 02 03               
                                                # 复位两个PWM舵机
                                                elif cmd[2] == 0x02 and cmd[3] == 0x03:
                                                    print('复位两个PWM舵机')
                                                    PWMServo.setServo(1, 1500, 100)
                                                    PWMServo.setServo(2, 1500, 100)
                                                    time.sleep(0.1)
                                                # 55 55 03 0F id
                                                # 读舵机的电压值
                                                elif cmd[2] == 0x03 and cmd[3] == 0x0F:
                                                    # 如果动作完成了
                                                    print('读取舵机的电压值')
                                                    if SSR.action_finish():
                                                        try:
                                                            time.sleep(0.05)
                                                            # ath.ceil 返回数字的上入整数
                                                            v = math.ceil(serial_servo_read_vin(cmd[4]))
                                                            print('电压值是:',v)
                                                            buf = [0x55, 0x55, 0x04, 0x0F, 0x00, 0x00]
                                                            buf[4] = v & 0xFF
                                                            buf[5] = (v >> 8) & 0xFF
                                                            # sendall 发送全部的数据
                                                            conn.sendall(bytearray(buf))
                                                            print('读取电压值成功')
                                                        except BaseException as e:
                                                            print(e)
                                                    else:
                                                        print('正在运行动作,请稍后')
                                           # 调试,打印命令cmd
                                            if DEBUG is True:
                                                for i in cmd:
                                                    print (hex(i))
                                                print('*' * 30)
                                        else: # 接收数据不正确
                                            break
                                    else: # 数据长度不够
                                        break
                                except Exception as e:   # 在recv_data 中搜不到 '\x55\x55'子串
                                    break
                            recv_data = b'' # 把数据缓冲区清空
                            action_times = None # 动作次数
                            action_num = None # 运行动作组名字
                        else:  # 接收数据大于13个  是错误的数据
                            recv_data = b'' # 把数据缓冲区清空
                            pass
                except Exception as e:  # 接收数据错误
                    print(e)
                    Flag = False # 退出循环
                    break
         
        # 数据发送完成,并且不再使用此客户端 断开此客户端连接
        # 在handle()方法之后被调用,执行当处理完请求后的清理工作,默认不会做任何事
        def finish(self):
    
           client_socket.remove(self.request)  # 从客户端列表中剔除此连接
           print("disconnected\tIP:"+self.ip+"\tPort:"+str(self.port))
    
    
    if __name__ == "__main__":
        server = LobotServer(("", 9029), LobotServerHandler)    # 建立服务器
        try:
            server.serve_forever()  # 开始服务器循环
        except Exception as e:
            print(e)
            server.shutdown() # 关闭服务器
            server.server_close() # 关闭服务
    
    
    
    展开全文
  • 在我们使用舵机的过程中,我们需要先对舵机进行配置,设置一下必要的参数,还要设置ID 文章目录一、配置舵机代码二、测试1.硬件连接2.测试 这部分在config_serial_servo文件里 一、配置舵机代码 # 配置串口舵机 #!/...
  • 我们总是需要让舵机去转动一定的角度,执行对应的动作。 文章目录前提:1.动作文件2.支持文件一、舵机运行代码二、测试 这一部分在Serial_Servo_Running文件里 前提: 因为这一部分需要运行动作组,所以需要动作文件...
  • STM32单线串口对总线舵机的控制

    万次阅读 多人点赞 2017-03-07 16:19:30
    1 总线舵机的介绍 总线伺服舵机即串行总线智能舵机,实际上可以理解为数字舵机的衍生品,数字舵机与模拟舵机相比而言是控制系统设计上的颠覆,而总线伺服舵机对于舵机而言则是在功能和运用上的颠覆。舵机的运用方式...
  • 摘要UART串行总线舵机需要配合UART串行总线舵机转接板使用,它的作用是将舵机的单线转换为双线TTL接口(Rx接收端,Tx发送端),通过双线TTL串口接口与单片机进行通信。或者通过转接板上的USB转TTL芯片(CH340),通过USB...
  • 2020年4月6日,深圳飞特模型有限公司发布了2020年新款磁编码版本的TTL串口总线舵机。这款舵机是基于SCS215电位器版本开发的更高性能的磁编码版本,不仅具备了飞特SM高端系列的高性价比功能,又具备了SCS系列的低价,...
  • 对于以下的舵机使用是协议为CAN总线协议,且功能强大 在经过转接头转换后,通过上位机与其通信,得到下方应答,且应答正确。 当然也可使用STM32单片机通过TJA1050芯片(芯片如下所示)与其通信 舵机如下所示 ....
  • 花了一晚上+一早上完成总线舵机的大部分常用指令编写,分享给大家,只要安装pyserial即可使用~ 文章目录总线舵机舵机优势驱动板总线协议驱动库 总线舵机 我买的是亚博智能他家的智能总线舵机,用来做机械臂。 挂个...
  • 幻尔lx15D 舵机三维图

    2020-06-16 15:29:26
    尔 串行总线舵机/机器人舵机/带反馈/高寿命高精度 LX-15D LX-15D舵机智能串行总线舵机LX-15D/机器人舵机/带反馈/高寿命高精度/幻尔
  • 最近要做一个机器人项目,上面需要用到32个舵机。如果是用普通的PWM舵机,每一个舵机各需要一根3PIN的连接线。那一个机器人下来光线都几十根了。而且有些地方的连线比较长,对抗干扰也不利。所以客户一开始就建议...
  • 网上买了块arduino的控制器,买了几个总线舵机装备做一个四足机器人,用店家给的控制程序发现只能实现匀速的舵机模式运动,想问一下如何编程算法可以实现舵机仿生的变速运动。
  • 舵机

    千次阅读 2019-07-01 13:33:02
    舵机基础知识 https://wenku.baidu.com/view/8902f4125f0e7cd18425361d.html 模拟舵机 PWM波控制, 控制周期20ms(反应周期20ms,易出现超调); 脉冲宽度0.5ms-2.5ms,对应-90°至+90°的位置; 占空比用来...
  • 在玩串口总线舵机时,我们需要用pigpio来驱动。因为我的系统带的有pigpio,所以我就以为不需要安装,谁知道,舵机怎么就是驱动运行不了。 在搜索了无数资料无果后,经过放弃,捡起,又放弃,又捡起这些过程。终于在...
  • 简介 本文主要目的是建立STM32与Dynamixel舵机间的通信连接,开发上位机——下位机——舵机的控制框架,在上位机下发指令,下位机执行舵机力控外...飞特总线舵机USB转RS485/TTL转接板(代替U2D2) 硬件连接 注意 RS4
  • 不要再对各种电机、舵机傻傻分不清楚了

    千次阅读 多人点赞 2020-08-03 17:54:36
    你是否和木木一样,想要自己diy一个机器人,却对电机、舵机、步进电机、伺服电机、数字舵机、模拟舵机、串行总线舵机……一大堆名词傻傻分不清楚?管你到底是啥,在我这都叫“马达”,哈哈哈哈哈!如果你也是这样...
  • Arduino六自由度总线机械臂组装,控制源码,技术资料,舵机选型,参数配置(新)
  • 数字舵机顾名思义就是用数字的信号去驱动,多数的为UART总线,通讯模式为一主多从,总线上可以挂载多个设备,每个设备都有固定的IP地址,只有收到匹配对应的地址的设备,后面的数据才有效。一共用了TX,VCC,GND三根线...
  • 舵机 - 什么是舵机

    2021-03-09 18:55:31
    上图是我们一般接触的舵机,常用于航模,机器人。舵机的英文名Servo Motors,即伺服电动机,如果我们在网上搜索这个词,大概率会出来以下几种外观的图。 这几种都属于伺服电动机,但我们说的舵机一般指第三种,那...
  • 无刷舵机和普通舵机,数字舵机,模拟舵机的区别 1.舵机的概念,组成和种类 概念: 舵机,是指在自动驾驶仪中操纵飞机舵面(操纵面)转动的一种执行部件。分有:①电动舵机,由电动机、传动部件和离合器组成。接受...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 615
精华内容 246
关键字:

总线舵机