Files
a7-can-sdk/examples/read_angles.py
T

38 lines
1.5 KiB
Python

"""Explicit read-only position queries. Default invocation does not open CAN."""
import argparse
import json
import time
from pathlib import Path
from a7_can_sdk import MotorBus
from a7_can_sdk.configuration import load
from a7_can_sdk.transport import SocketCan
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--config', type=Path)
parser.add_argument('--execute', action='store_true', help='Open CAN and send read-only queries')
parser.add_argument('--samples', type=int, default=10)
args = parser.parse_args()
if not args.execute:
print('Plan only: identify configured motors and query positions; no hardware connection.')
return
if args.config is None or args.samples < 1:
parser.error('--execute requires --config and --samples >= 1')
cfg = load(args.config)
bus = MotorBus(SocketCan(cfg['interface'], cfg['bitrate']), cfg['motors'], allow_control=False)
try:
bus.start()
for motor in cfg['motors']:
if bus.identify(motor['motor_id']) != motor['uid']:
raise RuntimeError('Motor identity mismatch')
for _ in range(args.samples):
rows = [{'motor_id': m['motor_id'], 'position_rad': bus.read_parameter(m['motor_id'], 'position')}
for m in cfg['motors']]
print(json.dumps({'host_time_ns': time.time_ns(), 'joints': rows}))
time.sleep(.1)
finally:
bus.close()
if __name__ == '__main__':
main()