K210, Openmv and serial bus servo communication (based on micropython) servo driver board and servo control board code

Bloggers are using it recentlyMagic Company When using serial bus servos,想使用k210控制,Because there is no official routine（The Raspberry Pi version ispython版本代碼,用不了）Hereby share the control code

主要調用函數

a.to_bytes(x,'little'/'big')
#將int型變成byte型
#The first parameter is the number of digits to display,The second parameter is the display order：從小到大/從大到小

int.from_bytes(a,'little'/'big')
#將byte型變成int型
#The first parameter is the number of digits to display,The second parameter is the display order：從小到大/從大到小

Servo driver board version

Note the baud rate115200

Here, the checksum is calculated one by onebyte轉成int相加,Instead of adding the parameters directly
例如servoWriteCmd(1,1,0,1000)
並非是
1+7+1+0+1000
but becomesbyte型
0x01+0x07+ 0x01 +0x00 +0x00+ 0xE8 +0x03 = 0xF4 = 244

0xF4取~後超過255So take the last byte1011即0x0B

So the final news is55 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(): #used for configurationIO口
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(): #Configure the single-wire serial port as output
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(): #Configure the single-wire serial port as input
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() #Configure the single-wire serial port as an output
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

Servo control board version

Note the baud rate9600

The code for the control board is relatively simple,Without a checksum it is only necessary to convert the corresponding parameter to byte型即可,Only the functions that control a single servo are written here,For the control of multiple servos, please write your own as appropriate

# 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() #Configure the single-wire serial port as an output
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