You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
124 lines
3.9 KiB
Python
124 lines
3.9 KiB
Python
#!/usr/bin/python3 -B
|
|
|
|
# Copyright 2021 Josh Pieper, jjp@pobox.com.
|
|
#
|
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
|
# you may not use this file except in compliance with the License.
|
|
# You may obtain a copy of the License at
|
|
#
|
|
# http://www.apache.org/licenses/LICENSE-2.0
|
|
#
|
|
# Unless required by applicable law or agreed to in writing, software
|
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
# See the License for the specific language governing permissions and
|
|
# limitations under the License.
|
|
|
|
|
|
"""
|
|
This example commands a single servo at ID #1 using the default
|
|
transport to hold the current position indefinitely, and prints the
|
|
state of the servo to the console.
|
|
"""
|
|
|
|
import asyncio
|
|
import math
|
|
import signal
|
|
import sys
|
|
from typing import Callable
|
|
|
|
import moteus
|
|
|
|
import logging
|
|
|
|
|
|
|
|
class BLDCMotor:
|
|
"""A BLDC Motor"""
|
|
|
|
def __init__(self) -> None:
|
|
self.controller = moteus.Controller()
|
|
|
|
async def set_stop(self, *args, **kwargs):
|
|
"""Stops the BLDC Motor"""
|
|
await self.controller.set_stop(*args, **kwargs)
|
|
|
|
async def set_position(self, *args, **kwargs):
|
|
state = await self.controller.set_position(*args, **kwargs)
|
|
return state
|
|
|
|
|
|
def create_async_signal_handler(coro_func: Callable[[], None]) -> Callable[[signal.Signals], None]:
|
|
async def _async_signal_handler(signal_type: signal.Signals) -> None:
|
|
logging.info(f"Received exit signal {signal.name}...")
|
|
|
|
await coro_func()
|
|
|
|
tasks = [t for t in asyncio.all_tasks() if t is not
|
|
asyncio.current_task()]
|
|
|
|
[task.cancel() for task in tasks]
|
|
|
|
logging.info(f"Cancelling {len(tasks)} outstanding tasks")
|
|
await asyncio.gather(*tasks)
|
|
|
|
loop = asyncio.get_event_loop()
|
|
loop.stop()
|
|
loop.close()
|
|
|
|
|
|
def _signal_handler(signal_type: signal.Signals, loop: asyncio.AbstractEventLoop) -> None:
|
|
print(f"Signal {signal_type.name} received.")
|
|
asyncio.create_task(_async_signal_handler(signal_type))
|
|
|
|
return _signal_handler
|
|
|
|
|
|
async def main(motor: BLDCMotor):
|
|
# In case the controller had faulted previously, at the start of
|
|
# this script we send the stop command in order to clear it.
|
|
await motor.set_stop()
|
|
|
|
while True:
|
|
# `set_position` accepts an optional keyword argument for each
|
|
# possible position mode register as described in the moteus
|
|
# reference manual. If a given register is omitted, then that
|
|
# register is omitted from the command itself, with semantics
|
|
# as described in the reference manual.
|
|
#
|
|
# The return type of 'set_position' is a moteus.Result type.
|
|
# It has a __repr__ method, and has a 'values' field which can
|
|
# be used to examine individual result registers.
|
|
state = await motor.set_position(position=math.nan, query=True)
|
|
|
|
# Print out everything.
|
|
print(state)
|
|
|
|
# Print out just the position register.
|
|
print("Position:", state.values[moteus.Register.POSITION])
|
|
|
|
# And a blank line so we can separate one iteration from the
|
|
# next.
|
|
print()
|
|
|
|
# Wait 20ms between iterations. By default, when commanded
|
|
# over CAN, there is a watchdog which requires commands to be
|
|
# sent at least every 100ms or the controller will enter a
|
|
# latched fault state.
|
|
await asyncio.sleep(0.02)
|
|
|
|
if __name__ == '__main__':
|
|
motor = BLDCMotor()
|
|
|
|
loop = asyncio.new_event_loop()
|
|
signal_handler = create_async_signal_handler(motor.set_stop)
|
|
|
|
# Register signal handlers for SIGNINT(Keyboard interrupt) and SIGTERM (Termination)
|
|
# for sig in (signal.SIGINT, signal.SIGTERM):
|
|
for sig in (signal.SIGINT):
|
|
loop.add_signal_handler(sig, signal_handler, sig, loop)
|
|
|
|
try:
|
|
loop.run_until_complete(main(motor))
|
|
finally:
|
|
loop.close() |