143 lines
4.5 KiB
Python
Executable File
143 lines
4.5 KiB
Python
Executable File
#!/opt/miniconda3/envs/py310/bin/python
|
|
import argparse
|
|
import json
|
|
import time
|
|
|
|
from ugv01 import UGV01Controller
|
|
|
|
|
|
def print_items(items) -> None:
|
|
if not items:
|
|
print("[]")
|
|
return
|
|
for item in items:
|
|
if isinstance(item, (dict, list)):
|
|
print(json.dumps(item, ensure_ascii=False, indent=2))
|
|
else:
|
|
print(item)
|
|
|
|
|
|
def add_common_serial_args(parser: argparse.ArgumentParser) -> None:
|
|
parser.add_argument("--port", default="/dev/ttyS7")
|
|
parser.add_argument("--baudrate", type=int, default=115200)
|
|
parser.add_argument("--timeout", type=float, default=0.3)
|
|
|
|
|
|
def build_parser() -> argparse.ArgumentParser:
|
|
parser = argparse.ArgumentParser(description="UGV01 UART control CLI")
|
|
sub = parser.add_subparsers(dest="command", required=True)
|
|
|
|
imu = sub.add_parser("imu", help="Read IMU data")
|
|
add_common_serial_args(imu)
|
|
|
|
base_feedback = sub.add_parser("base-feedback", help="Read chassis feedback")
|
|
add_common_serial_args(base_feedback)
|
|
|
|
speed = sub.add_parser("speed", help="Set left/right wheel speed")
|
|
add_common_serial_args(speed)
|
|
speed.add_argument("--left", type=float, required=True)
|
|
speed.add_argument("--right", type=float, required=True)
|
|
speed.add_argument("--duration", type=float, default=0.0)
|
|
|
|
pwm = sub.add_parser("pwm", help="Set left/right wheel PWM")
|
|
add_common_serial_args(pwm)
|
|
pwm.add_argument("--left", type=int, required=True)
|
|
pwm.add_argument("--right", type=int, required=True)
|
|
pwm.add_argument("--duration", type=float, default=0.0)
|
|
|
|
ros = sub.add_parser("ros", help="ROS style linear/angular control")
|
|
add_common_serial_args(ros)
|
|
ros.add_argument("--x", type=float, required=True)
|
|
ros.add_argument("--z", type=float, required=True)
|
|
ros.add_argument("--duration", type=float, default=0.0)
|
|
|
|
oled_text = sub.add_parser("oled-text", help="Set OLED line text")
|
|
add_common_serial_args(oled_text)
|
|
oled_text.add_argument("--line", type=int, required=True)
|
|
oled_text.add_argument("--text", required=True)
|
|
|
|
oled_restore = sub.add_parser("oled-restore", help="Restore OLED default content")
|
|
add_common_serial_args(oled_restore)
|
|
|
|
serial_feedback = sub.add_parser("serial-feedback", help="Toggle continuous feedback")
|
|
add_common_serial_args(serial_feedback)
|
|
serial_feedback.add_argument("--enable", action="store_true")
|
|
|
|
serial_echo = sub.add_parser("serial-echo", help="Toggle serial echo")
|
|
add_common_serial_args(serial_echo)
|
|
serial_echo.add_argument("--enable", action="store_true")
|
|
|
|
raw = sub.add_parser("raw", help="Send raw JSON payload")
|
|
add_common_serial_args(raw)
|
|
raw.add_argument("--payload", required=True, help='Example: {"T":126}')
|
|
raw.add_argument("--expect-response", action="store_true")
|
|
|
|
return parser
|
|
|
|
|
|
def main() -> int:
|
|
args = build_parser().parse_args()
|
|
with UGV01Controller(
|
|
port=args.port,
|
|
baudrate=args.baudrate,
|
|
timeout=args.timeout,
|
|
) as car:
|
|
if args.command == "imu":
|
|
print_items(car.imu())
|
|
return 0
|
|
|
|
if args.command == "base-feedback":
|
|
print_items(car.base_feedback())
|
|
return 0
|
|
|
|
if args.command == "speed":
|
|
car.speed(args.left, args.right)
|
|
if args.duration > 0:
|
|
time.sleep(args.duration)
|
|
car.stop()
|
|
return 0
|
|
|
|
if args.command == "pwm":
|
|
car.pwm(args.left, args.right)
|
|
if args.duration > 0:
|
|
time.sleep(args.duration)
|
|
car.pwm(0, 0)
|
|
return 0
|
|
|
|
if args.command == "ros":
|
|
car.ros_ctrl(args.x, args.z)
|
|
if args.duration > 0:
|
|
time.sleep(args.duration)
|
|
car.ros_ctrl(0.0, 0.0)
|
|
return 0
|
|
|
|
if args.command == "oled-text":
|
|
car.oled_text(args.line, args.text)
|
|
return 0
|
|
|
|
if args.command == "oled-restore":
|
|
car.oled_restore()
|
|
return 0
|
|
|
|
if args.command == "serial-feedback":
|
|
car.serial_feedback(args.enable)
|
|
return 0
|
|
|
|
if args.command == "serial-echo":
|
|
car.serial_echo(args.enable)
|
|
return 0
|
|
|
|
if args.command == "raw":
|
|
payload = json.loads(args.payload)
|
|
if args.expect_response:
|
|
print_items(car.request(payload))
|
|
else:
|
|
car.send(payload)
|
|
return 0
|
|
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|