80 lines
2.2 KiB (Stored with Git LFS)
Python
80 lines
2.2 KiB (Stored with Git LFS)
Python
import argparse
|
|
import time
|
|
from gobotrpc import GobotRPC_PackageParseError
|
|
from gobotrpc.rpc_packages import GobotRPCVacumRequest
|
|
from gobotrpc.rpc_packages_head import GobotRPCDropStoneRequest, GobotRPCMoveZAxisRequest, GobotHeadZAxisPosition
|
|
from gobotrpc.rpc_packages_corexy import (
|
|
GobotRPCHomeRequest,
|
|
GobotRPCHomeResponse,
|
|
GobotRPCReleaseMotorsRequest,
|
|
GobotRPCReleaseMotorsResponse,
|
|
GobotRPCSetPaddingRequest,
|
|
GobotRPCSetPaddingResponse,
|
|
GobotRPCGotoRequest,
|
|
GobotRPCGotoResponse,
|
|
)
|
|
|
|
from uart_interface.serial import GobotRPC_CI_Hardware_Serial
|
|
from uart_interface.ci_highLevel import GobotRPC_CI
|
|
from uart_interface.ci_packages import CMDS, CI_ParserError
|
|
|
|
import threading
|
|
|
|
# Args parser
|
|
parser = argparse.ArgumentParser(description='GobotRPC')
|
|
parser.add_argument('port', type=str, help='Serial port', default="/dev/ttyACM1")
|
|
|
|
|
|
def main():
|
|
args = parser.parse_args()
|
|
|
|
gobotrpc_ci_hardware = GobotRPC_CI_Hardware_Serial(args.port, 115200)
|
|
gobotrpc_ci = GobotRPC_CI(gobotrpc_ci_hardware)
|
|
|
|
def rxThreadFn():
|
|
while True:
|
|
try:
|
|
ci_package = gobotrpc_ci.rx_ci_package()
|
|
except GobotRPC_PackageParseError as e:
|
|
print(e)
|
|
continue
|
|
except CI_ParserError as e:
|
|
print(e)
|
|
continue
|
|
|
|
if ci_package.get_cmd() != CMDS.HEARTBEAT:
|
|
print(ci_package)
|
|
|
|
rxThread = threading.Thread(target=rxThreadFn)
|
|
rxThread.daemon = True
|
|
rxThread.start()
|
|
|
|
for i in range(4):
|
|
gobotrpc_ci.setAddrPortMap(0x23, i)
|
|
|
|
|
|
gobotrpc_ci.resetAdapter()
|
|
time.sleep(0.5)
|
|
gobotrpc_ci.node_get_info(0x23)
|
|
|
|
input("SetPadding>")
|
|
gobotrpc_ci.send_gobotrpc_package(GobotRPCSetPaddingRequest(
|
|
(1000, 2000), (5000, 6000), (200, 300), 3, 3
|
|
), 0x23)
|
|
|
|
input("SetPadding>")
|
|
gobotrpc_ci.send_gobotrpc_package(GobotRPCHomeRequest(), 0x23)
|
|
|
|
while True:
|
|
|
|
x = int(input("X>") or "0")
|
|
y = int(input("Y>") or "0")
|
|
gobotrpc_ci.send_gobotrpc_package(
|
|
GobotRPCGotoRequest(x, y, False), 0x23
|
|
)
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|