38 lines
1.5 KiB
Python
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()
|