K210、Openmv与串行总线舵机通信（基于micropython）舵机驱动板和舵机控制板代码_舵机驱动板代码

最近博主在使用幻尔公司 串行总线舵机时，想使用k210控制（openmv和k210都是micropython编写的所以这个代码是通用的），由于官方没有相关例程（树莓派的版本是python版本代码，用不了）特此分享一下控制代码

主要调用函数

a.to_bytes(x,'little'/'big')
#将int型变成byte型
#第一个参数是显示位数，第二个参数为显示顺序：从小到大/从大到小
1
2
3

int.from_bytes(a,'little'/'big')
#将byte型变成int型
#第一个参数是显示位数，第二个参数为显示顺序：从小到大/从大到小
1
2
3

舵机驱动板版本

需要注意波特率为115200

这里计算校验和要逐个byte转成int相加，不是直接把各参数相加
例如servoWriteCmd(1,1,0,1000)
并非是
1+7+1+0+1000
而是化为byte型
0x01+0x07+ 0x01 +0x00 +0x00+ 0xE8 +0x03 = 0xF4 = 244

0xF4取~后超过255因此取最后字节1011即0x0B

故最后的消息为55 55 01 07 01 00 00 E8 03 0B

from machine import UART,Timer
from Maix import GPIO
from fpioa_manager import fm
from ubinascii import *
import time
#映射串口引脚

#初始化串口
uart = UART(UART.UART1, 115200, read_buf_len=4096)


def servoWriteCmd(id, cmd, par1, par2):
    fm.register(6, fm.fpioa.UART1_RX, force=True)
    fm.register(7, fm.fpioa.UART1_TX, force=True)
    begin = 85   #0x55的十进制
    buf= begin.to_bytes(1,'little')
    buf += buf
    try:
        len = 7
        sum = 0
        a = id.to_bytes(1,'little')
        b = len.to_bytes(1,'little')
        c = cmd.to_bytes(1,'little')
        d = par1.to_bytes(1,'little')
        dd = par1.to_bytes(1,'big')
        f = par2.to_bytes(2,'little')
        ff = par2.to_bytes(2,'big')
        #print(par2 % 256)
        sum = int.from_bytes(a,'big')+ int.from_bytes(b,'big')+ int.from_bytes(c,'big')+(par1 % 256)+(par1 // 256)+(par2 % 256)+(par2 // 256)
        print(sum)
        sum = ~sum  #取反
        print(sum.to_bytes(1,'little'))
        buf += id.to_bytes(1,'little')+ len.to_bytes(1,'little') + cmd.to_bytes(1,'little')+par1.to_bytes(2,'little')+par2.to_bytes(2,'little')+sum.to_bytes(1,'little')
        uart.write(buf)
        print(buf)

    except Exception as e:
        print(e)

def portInit(): #配置用到的IO口
    fm.register(6, fm.fpioa.GPIO0, force=True)
    RX = GPIO(GPIO.GPIO0,GPIO.OUT)
    RX.value(0)
    fm.register(7, fm.fpioa.GPIO1, force=True)
    TX = GPIO(GPIO.GPIO1,GPIO.OUT)
    TX.value(1)

def portWrite():  #配置单线串口为输出
    fm.register(6, fm.fpioa.GPIO0, force=True)
    RX = GPIO(GPIO.GPIO0,GPIO.OUT)
    RX.value(0)
    fm.register(7, fm.fpioa.GPIO1, force=True)
    TX = GPIO(GPIO.GPIO1,GPIO.OUT)
    TX.value(1)


def portRead():   #配置单线串口为输入
    fm.register(6, fm.fpioa.GPIO0, force=True)
    RX = GPIO(GPIO.GPIO0,GPIO.OUT)
    RX.value(1)
    fm.register(7, fm.fpioa.GPIO1, force=True)
    TX = GPIO(GPIO.GPIO1,GPIO.OUT)
    TX.value(0)

portInit()
while True:
    try:
        portWrite() #将单线串口配置为输出
        servoWriteCmd(1,1,0,1000) #发送命令 参数1 舵机id=1, 参数2 命令 = 1, 参数3 位置 = 0, 参数4 时间 = 1000ms  55 55 01 07 01 00 00 E8 03 0B
        time.sleep(1.1)
        servoWriteCmd(1,1,1000,2000)#55 55 01 07 01 E8 03 D0 07 34
        time.sleep(2.1)
    except Exception as e:
        print(e)
        break
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

舵机控制板版本

需要注意波特率为9600

控制板的代码相对简单，没有校验和只需要将相应参数转换为byte型即可，这里只写了控制单个舵机的函数，多个舵机控制读者请酌情自行编写

# Untitled - By: lenovo - 周一 8月 1 2022

from machine import UART,Timer
from Maix import GPIO
from fpioa_manager import fm
from ubinascii import *
import time
#映射串口引脚

#初始化串口
uart = UART(UART.UART1, 9600, read_buf_len=4096)


def servoWriteCmd(cmd,count,par1,id,par2):
    fm.register(6, fm.fpioa.UART1_RX, force=True)
    fm.register(7, fm.fpioa.UART1_TX, force=True)
    begin = 85
    buf= begin.to_bytes(1,'little')
    buf += buf
    try:
        len = count*3+5
        buf += len.to_bytes(1,'little')+ cmd.to_bytes(1,'little') + count.to_bytes(1,'little')+par1.to_bytes(2,'little')+id.to_bytes(1,'little')+par2.to_bytes(2,'little')
        uart.write(buf)
        print(buf)

    except Exception as e:
        print(e)

#portInit()
while True:
    try:
        #portWrite() #将单线串口配置为输出
        servoWriteCmd(3,1,1000,2,800) #发送命令 参数1 舵机id=1, 参数2 命令 = 1, 参数3 位置 = 0, 参数4 时间 = 1000ms 
        time.sleep(1.1)
        servoWriteCmd(3,1,1000,2,100)#
        time.sleep(2.1)
    except Exception as e:
        print(e)
        break

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40