亚博microROS 机器人配置与控制

硬件组装：

参见官网。

1、机器人组装

配置参数：

config_robot.py

import serial
import struct
import time

'''
Author  : Yahboom Team
Version : V1.0.1
LastEdit: 2024.02.18
'''


"""
ORDER 用来存放命令地址和对应数据
ORDER is used to store the command address and corresponding data
"""
ORDER = {
    
    "WIFI_SSID": [0x01],
    "WIFI_PASSWD": [0x02],
    "AGENT_IP": [0x03, 0, 0, 0, 0],
    "AGENT_PORT": [0x04, 0, 0],
    "CAR_TYPE": [0x05, 0, 0],
    "DOMAIN_ID": [0x06, 0, 0],
    "SERIAL_BAUDRATE": [0x07, 0, 0, 0, 0],
    "SERVO_OFFSET": [0x08, 0, 0],
    "MOTOR_PID": [0x09, 0, 0, 0, 0, 0, 0],
    "IMU_YAW_PID": [0x0A, 0, 0, 0, 0, 0, 0],
    "ROS_NAMESPACE": [0x0B],

    "REBOOT_DEVICE": [0x20, 0x00, 0X00],
    "RESET_CONFIG": [0x21, 0x00, 0X00],

    "REQUEST_DATA": [0x50, 0, 0],
    "FIRMWARE_VERSION": [0x51],
}


class MicroROS_Robot():

    def __init__(self, port="/dev/ttyUSB0", debug=False):
        self.__ser = serial.Serial(port, 115200, timeout=0.05)
        self.__rx_FLAG = 0
        self.__rx_COUNT = 0
        self.__rx_ADDR = 0
        self.__rx_LEN = 0
        self.__RX_BUF_LEN_MAX = 40
        self.__rx_DATA = bytearray(self.__RX_BUF_LEN_MAX)
        self.__send_delay = 0.01
        self.__read_delay = 0.01
        self.__debug = debug

        self.__HEAD = 0xFF
        self.__DEVICE_ID = 0xF8
        self.__RETURN_ID = 0xF7
        self.__READ_DATA = 0x50

        self.AGENT_TYPE_WIFI_UDP = 0
        self.AGENT_TYPE_SERIAL = 1

        self.CAR_TYPE_COMPUTER = 0
        self.CAR_TYPE_RPI5 = 1

    # 发送一些数据到设备 Send some data to device
    def __send(self, key, len=1):
        order = ORDER[key][0]
        value = []
        value_sum = 0
        for i in range(0, len):
            value.append(ORDER[key][1 + i])
            value_sum = value_sum + ORDER[key][1 + i]
        sum_data = (self.__HEAD + self.__DEVICE_ID + (len + 0x05) + order + value_sum) % 256
        tx = [self.__HEAD, self.__DEVICE_ID, (len + 0x05), order]
        tx.extend(value)
        tx.append(sum_data)
        self.__ser.write(tx)
        if self.__send_delay > 0:
            time.sleep(self.__send_delay)
        if self.__debug:
            print ("Send: [0x" + ', 0x'.join('{:02X}'.format(x) for x in tx) + "]")
            # print ("Send: [" + ' '.join('{:02X}'.format(x) for x in tx) + "]")

    # 发送请求数据 Send request data
    def __request(self, addr, param=0):
        order = self.__READ_DATA
        buf_len = 7
        sum_data = (self.__HEAD + self.__DEVICE_ID + buf_len + order + addr + param) % 256
        tx = [self.__HEAD, self.__DEVICE_ID, buf_len, order, addr, param, sum_data]
        self.__ser.flushInput()
        self.__ser.flushOutput()
        for i in range(self.__RX_BUF_LEN_MAX):
            self.__rx_DATA[i] = 0
        self.__ser.write(tx)
        if self.__debug:
            print ("Read: [0x" + ', 0x'.join('{:02X}'.format(x) for x in tx) + "]")
            # print ("Read: [" + ' '.join('{:02X}'.format(x) for x in tx) + "]")

    # 解析数据 parse data
    def __unpack(self):
        n = self.__ser.inWaiting()
        rx_CHECK = 0
        if n:
            #print("OK")
            data_array = self.__ser.read_all()
            if self.__debug:
                # print("rx_data:", list(data_array))
                print ("rx_data: [0x" + ', 0x'.join('{:02X}'.format(x) for x in data_array) + "]")
                # print ("rx_data: [" + ' '.join('{:02X}'.format(x) for x in data_array) + "]")
            for data in data_array:
                if self.__rx_FLAG == 0:
                    if data == self.__HEAD:
                        self.__rx_FLAG = 1
                    else:
                        self.__rx_FLAG = 0

                elif self.__rx_FLAG == 1:
                    if data == self.__RETURN_ID:
                        self.__rx_FLAG = 2
                    else:
                        self.__rx_FLAG = 0

                elif self.__rx_FLAG == 2:
                    self.__rx_LEN = data
                    self.__rx_FLAG = 3