树莓派读取舵机数据与驱动

舵机数据读取

Read.py

#!/usr/bin/python3
#幻尔科技总线舵机通信#
import serial
import pigpio
import time

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
LOBOT_SERVO_ID_READ              = 14
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
LOBOT_SERVO_VIN_LIMIT_READ       = 23
LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE = 24
LOBOT_SERVO_TEMP_MAX_LIMIT_READ  = 25
LOBOT_SERVO_TEMP_READ            = 26
LOBOT_SERVO_VIN_READ             = 27
LOBOT_SERVO_POS_READ             = 28
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
LOBOT_SERVO_LED_CTRL_READ        = 34
LOBOT_SERVO_LED_ERROR_WRITE      = 35
LOBOT_SERVO_LED_ERROR_READ       = 36


pi = pigpio.pi()  #初始化 pigpio库
serialHandle = serial.Serial("/dev/ttyAMA0", 115200)  #初始化串口， 波特率为115200

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(17, 1) #拉高RX_CON 即 GPIO17
    pi.write(27, 0) #拉低TX_CON 即 GPIO27




def checksum(buf):
    # 计算校验和
    sum = 0x00
    for b in buf:  # 求和
        sum += b
    sum = sum - 0x55 - 0x55  # 去掉命令开头的两个 0x55
    sum = ~sum  # 取反
    return sum & 0xff

def serial_serro_wirte_cmd(id=None, w_cmd=None, dat1=None, dat2=None):
    '''
    写指令
    :param id:
    :param w_cmd:
    :param dat1:
    :param dat2:
    :return:
    '''
    portWrite()
    buf = bytearray(b'\x55\x55')  # 帧头
    buf.append(id)
    # 指令长度
    if dat1 is None and dat2 is None:
        buf.append(3)
    elif dat1 is not None and dat2 is None:
        buf.append(4)
    elif dat1 is not None and dat2 is not None:
        buf.append(7)

    buf.append(w_cmd)  # 指令
    # 写数据
    if dat1 is None and dat2 is None:
        pass
    elif dat1 is not None and dat2 is None:
        buf.append(dat1 & 0xff)  # 偏差
    elif dat1 is not None and dat2 is not None:
        buf.extend([(0xff & dat1), (0xff & (dat1 >> 8))])  # 分低8位 高8位 放入缓存
        buf.extend([(0xff & dat2), (0xff & (dat2 >> 8))])  # 分低8位 高8位 放入缓存
    # 校验和
    buf.append(checksum(buf))
    # for i in buf:
    #     print('%x' %i)
    serialHandle.write(buf)  # 发送

def serial_servo_read_cmd(id=None, r_cmd=None):
    '''
    发送读取命令
    :param id:
    :param r_cmd:
    :param dat:
    :return:
    '''
    portWrite()
    buf = bytearray(b'\x55\x55')  # 帧头
    buf.append(id)
    buf.append(3)  # 指令长度
    buf.append(r_cmd)  # 指令
    buf.append(checksum(buf))  # 校验和
    serialHandle.write(buf)  # 发送
    time.sleep(0.00034)#小延时，等命令发送完毕。不知道是否能进行这么精确的延时的，但是修改这个值的确实会产生影响。
                         #这个值效果最好，网上查值的跟这个值也接近

#读取ID
def readID(cmd):
    portWrite() #将单线串口配置为输出
    serialHandle.flushInput() #清空接收缓存
    serial_servo_read_cmd(cmd, LOBOT_SERVO_ID_READ)
    #time.sleep(0.00034)  
    portRead() #将单线串口配置为输入
    time.sleep(0.005)  #稍作延时，等待接收完毕
    count = serialHandle.inWaiting() #获取接收缓存中的字节数
    id = None
    if count != 0: #如果接收到的数据不空
        recv_data = serialHandle.read(count) #读取接收到的数据
        if count == 7: #如果接收到的数据是8个字节（符合位置读取命令的返回数据的长度）
            if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == LOBOT_SERVO_ID_READ:
                #第一第二个字节等于0x55, 第5个字节是0x1C 就是 28 就是 位置读取命令的命令号
                id=0xffff & (recv_data[5] | (0xff00 & (0x00 << 8)))  
                 #上面这小段代码简化了操作没有进行和校验，只要帧头和命令对了就认为数据无误
            
    

        return id  #返回读取到的位置
 
#dve读偏差
def readDve(cmd):
    portWrite() #将单线串口配置为输出
    serialHandle.flushInput() #清空接收缓存
    serial_servo_read_cmd(cmd, LOBOT_SERVO_ANGLE_OFFSET_READ)
    #time.sleep(0.00034)  
    portRead() #将单线串口配置为输入
    time.sleep(0.005)  #稍作延时，等待接收完毕
    count = serialHandle.inWaiting() #获取接收缓存中的字节数
    dve = None
    if count != 0: #如果接收到的数据不空
        recv_data = serialHandle.read(count) #读取接收到的数据
        if count == 7: #如果接收到的数据是8个字节（符合位置读取命令的返回数据的长度）
            
            if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == LOBOT_SERVO_ANGLE_OFFSET_READ:
                #第一第二个字节等于0x55, 第5个字节是0x1C 就是 28 就是 位置读取命令的命令号
                
                dve=0xffff & (recv_data[5] | (0xff00 & (0x00 << 8))) #将接收到的字节数据拼接成完整的位置数据  
                #上面这小段代码简化了操作没有进行和校验，只要帧头和命令对了就认为数据无误
            
    

        return dve  #返回读取到的位置



##pos读位置
##
def readPos(cmd):
    portWrite() #将单线串口配置为输出
    serialHandle.flushInput() #清空接收缓存
    serial_servo_read_cmd(cmd, LOBOT_SERVO_POS_READ)
    #time.sleep(0.00034)  
    portRead() #将单线串口配置为输入
    time.sleep(0.005)  #稍作延时，等待接收完毕
    count = serialHandle.inWaiting() #获取接收缓存中的字节数
    pos = None
    if count != 0: #如果接收到的数据不空
        recv_data = serialHandle.read(count) #读取接收到的数据
        if count == 8: #如果接收到的数据是8个字节（符合位置读取命令的返回数据的长度）
            if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == LOBOT_SERVO_POS_READ:
                #第一第二个字节等于0x55, 第5个字节是0x1C 就是 28 就是 位置读取命令的命令号
                 pos= 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8))) #将接收到的字节数据拼接成完整的位置数据
                 #上面这小段代码简化了操作没有进行和校验，只要帧头和命令对了就认为数据无误
                 
   
      
    return pos  #返回读取到的位置


##vin读电压
def readVin(cmd):
    portWrite() #将单线串口配置为输出
    serialHandle.flushInput() #清空接收缓存
    serial_servo_read_cmd(cmd, LOBOT_SERVO_VIN_READ)
    #time.sleep(0.00034)  
    portRead() #将单线串口配置为输入
    time.sleep(0.005)  #稍作延时，等待接收完毕
    count = serialHandle.inWaiting() #获取接收缓存中的字节数
    vin = None
    if count != 0: #如果接收到的数据不空
        recv_data = serialHandle.read(count) #读取接收到的数据
        if count == 8: #如果接收到的数据是8个字节（符合位置读取命令的返回数据的长度）
            if recv_data[0] == 0x55 and recv_data[1] == 0x55 and recv_data[4] == LOBOT_SERVO_VIN_READ:
                #第一第二个字节等于0x55, 第5个字节是0x1C 就是 28 就是 位置读取命令的命令号
                 vin= 0xffff & (recv_data[5] | (0xff00 & (recv_data[6] << 8))) #将接收到的字节数据拼接成完整的位置数据
                 #上面这小段代码简化了操作没有进行和校验，只要帧头和命令对了就认为数据无误
                 

    return vin 


serial_servo_read_cmd(254, LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE)#所有舵机掉电
while True:
    try:
        
        readID(254)
        id = readID(254) #读取id
        print('id:',id) #打印id
        time.sleep(0.5)
        
        readPos(1)
        pos = readPos(1) #获取1号舵机的位置
        print('pos:',pos) #打印位置
        time.sleep(0.5)
        
        readDve(1)#读取偏差
        dve= readDve(1)
        print('dve:',dve) #打印偏差
        time.sleep(0.5)
        
        readVin(1)#读取电压
        vin=readVin(1)
        print('vin(mv):',vin)#打印偏差
        
        time.sleep(1) #延时1秒
         
        
    except Exception as e:
        print(e)
        break

        ###！！！！！因为多个数据一起读取，读取间隔不好控制，会容易出现数据混乱，读取时数据不对会返回None

现象：

舵机驱动

Move.py

import time
import ctypes
import serial
import pigpio
#幻尔科技总线舵机通信#

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
LOBOT_SERVO_ID_READ              = 14
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
LOBOT_SERVO_VIN_LIMIT_READ       = 23
LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE = 24
LOBOT_SERVO_TEMP_MAX_LIMIT_READ  = 25
LOBOT_SERVO_TEMP_READ            = 26
LOBOT_SERVO_VIN_READ             = 27
LOBOT_SERVO_POS_READ             = 28
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
LOBOT_SERVO_LED_CTRL_READ        = 34
LOBOT_SERVO_LED_ERROR_WRITE      = 35
LOBOT_SERVO_LED_ERROR_READ       = 36

serialHandle = serial.Serial("/dev/ttyAMA0", 115200)  # 初始化串口， 波特率为115200

pi = pigpio.pi()  #初始化 pigpio库


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(17, 1) #拉高RX_CON 即 GPIO17
    pi.write(27, 0) #拉低TX_CON 即 GPIO27


def checksum(buf):
    # 计算校验和
    sum = 0x00
    for b in buf:  # 求和
        sum += b
    sum = sum - 0x55 - 0x55  # 去掉命令开头的两个 0x55
    sum = ~sum  # 取反
    return sum & 0xff

def serial_serro_wirte_cmd(id=None, w_cmd=None, dat1=None, dat2=None):
    '''
    写指令
    :param id:
    :param w_cmd:
    :param dat1:
    :param dat2:
    :return:
    '''
    portWrite()
    buf = bytearray(b'\x55\x55')  # 帧头
    buf.append(id)
    # 指令长度
    if dat1 is None and dat2 is None:
        buf.append(3)
    elif dat1 is not None and dat2 is None:
        buf.append(4)
    elif dat1 is not None and dat2 is not None:
        buf.append(7)

    buf.append(w_cmd)  # 指令
    # 写数据
    if dat1 is None and dat2 is None:
        pass
    elif dat1 is not None and dat2 is None:
        buf.append(dat1 & 0xff)  # 偏差
    elif dat1 is not None and dat2 is not None:
        buf.extend([(0xff & dat1), (0xff & (dat1 >> 8))])  # 分低8位 高8位 放入缓存
        buf.extend([(0xff & dat2), (0xff & (dat2 >> 8))])  # 分低8位 高8位 放入缓存
    # 校验和
    buf.append(checksum(buf))
    # for i in buf:
    #     print('%x' %i)
    serialHandle.write(buf)  # 发送

def serial_servo_read_cmd(id=None, r_cmd=None):
    '''
    发送读取命令
    :param id:
    :param r_cmd:
    :param dat:
    :return:
    '''
    portWrite()
    buf = bytearray(b'\x55\x55')  # 帧头
    buf.append(id)
    buf.append(3)  # 指令长度
    buf.append(r_cmd)  # 指令
    buf.append(checksum(buf))  # 校验和
    serialHandle.write(buf)  # 发送
    time.sleep(0.00034)




def setBusServoPulse(id, pulse, use_time):
    """
    驱动串口舵机转到指定位置
    :param id: 要驱动的舵机id
    :pulse: 位置
    :use_time: 转动需要的时间
    """

    pulse = 0 if pulse < 0 else pulse
    pulse = 1000 if pulse > 1000 else pulse
    use_time = 0 if use_time < 0 else use_time
    use_time = 30000 if use_time > 30000 else use_time
    serial_serro_wirte_cmd(id, LOBOT_SERVO_MOVE_TIME_WRITE, pulse, use_time)
    return pulse  #x新增反馈可以删除
  
while True:
      setBusServoPulse(id=254, pulse=1000, use_time=1000)
      time.sleep(1) # 延时1s
      setBusServoPulse(id=254, pulse=0, use_time=1000)
      time.sleep(1) # 延时1s

树莓派舵机驱动