返回教程正文

配套源码

main.py

app/python/main.py
zdt_x57s_can Brick：把一台 ZDT X57S 封装成可复用对象app/python/main.py
Python69 行
  1. # SPDX-License-Identifier: MIT
  3. import time
  5. from arduino.app_utils import App
  6. from zdt_x57s_can import ZdtX57SCan, ZdtX57SCanError
  9. MOTOR_IDS = (1, 2, 3, 4)
  10. motors = tuple(ZdtX57SCan(motor_id) for motor_id in MOTOR_IDS)
  11. probe_complete = False
  12. next_probe_time = 0.0
  13. last_error = None
  16. def read_all_motor_speeds():
  17. """
  18. @description : 由测试App组合四个单电机对象并依次读取实时速度
  19. @param : 无参数
  20. @return : 电机地址到实时RPM的字典
  21. """
  22. speeds = {}
  23. for motor in motors:
  24. try:
  25. speeds[motor.motor_id] = motor.read_speed()
  26. except ZdtX57SCanError as error:
  27. raise ZdtX57SCanError(
  28. f"motor {motor.motor_id}: {error}"
  29. ) from error
  30. return speeds
  33. def loop():
  34. """
  35. @description : 等待Linux原生CAN网关就绪并对1至4号电机执行一次无运动探测
  36. @param : 无参数
  37. @return : 无返回值
  38. """
  39. global probe_complete, next_probe_time, last_error
  41. current_time = time.monotonic()
  42. if not probe_complete and current_time >= next_probe_time:
  43. try:
  44. speeds = read_all_motor_speeds()
  45. print(
  46. "[zdt-x57s] 四电机CAN通信正常: "
  47. + ", ".join(
  48. f"id{motor_id}={speeds[motor_id]}RPM"
  49. for motor_id in MOTOR_IDS
  50. ),
  51. flush=True,
  52. )
  53. probe_complete = True
  54. last_error = None
  55. except ZdtX57SCanError as error:
  56. error_text = str(error)
  57. if error_text != last_error:
  58. print(
  59. "[zdt-x57s] 等待CAN网关或电机应答: " + error_text,
  60. flush=True,
  61. )
  62. last_error = error_text
  63. next_probe_time = current_time + 3.0
  65. time.sleep(0.05)
  68. App.run(user_loop=loop)