def __init__(self, args):
self.args = args
qr = moteus.QueryResolution()
qr.position = 3
qr.velocity = 3
qr.torque = 3
self.drive = moteus.Controller(id=1, query_resolution=qr)
self.cut = moteus.Controller(id=2)
async def main():
c = moteus.Controller()
await c.set_stop() # in case there was a fault
while True:
print(await c.set_position(position=math.nan, query=True))
await asyncio.sleep(0.02)
def __init__(self,
number,
transport,
console,
prefix,
config_tree_item,
data_tree_item,
can_prefix=None):
self.error_count = 0
self.poll_count = 0
self.number = number
self.controller = moteus.Controller(number, can_prefix=can_prefix)
self._transport = transport
self._stream = DeviceStream(transport, self.controller)
self._console = console
self._prefix = prefix
self._config_tree_item = config_tree_item
self._data_tree_item = data_tree_item
self._telemetry_records = {}
self._schema_name = None
self._config_tree_items = {}
self._config_callback = None
self._updating_config = False
def __init__(
self, id
): # NOTE: instantiate your own transport instance when using rpi CAN-FD interface
self.id = id
self.controller = moteus.Controller(id=id)
self.stream = moteus.Stream(
self.controller
) # moteus diagnostic stream, do NOT use for real time
def __init__(self):
#self.args = args
# for now, let's just use the default query res of F32
qr = moteus.QueryResolution()
# hp_motor = hip pitch motor
self.m_ctrlr = moteus.Controller(id=1)
self.pos_tolerance = 0.05
async def main():
c = moteus.Controller()
await c.set_stop() # in case there was a fault
while True:
print(await c.set_position(position=math.nan,
velocity=0.5,
maximum_torque=0.5,
stop_position=0.9,
query=True))
await asyncio.sleep(0.02)
async def test_task(self):
c = moteus.Controller()
await c.set_stop() # in case there was a fault
while True:
loop = asyncio.get_event_loop()
self.running = True
print(await c.set_position(
position=math.nan,
velocity=0.5,
maximum_torque=0.5,
stop_position=0.9,
query=True
)) #commande pour faire actioer position et retour indfo console
await asyncio.sleep(0.02, loop)
self.running = False
def __init__(self, number, transport, console, prefix, config_tree_item,
data_tree_item):
self.error_count = 0
self.poll_count = 0
self.number = number
self.controller = moteus.Controller(number)
self._transport = transport
self._stream = DeviceStream(transport, self.controller)
self._console = console
self._prefix = prefix
self._config_tree_item = config_tree_item
self._data_tree_item = data_tree_item
self._serial_state = self.STATE_LINE
self._telemetry_records = {}
self._schema_name = None
self._config_tree_items = {}
self._config_callback = None
self._start_time = None
def __init__(self, knee, hip_pitch):
self.knee = knee
self.hip_pitch = hip_pitch
# servo_bus_map arg describes which IDs are found on which bus
self.transport = moteus_pi3hat.Pi3HatRouter(servo_bus_map={
1: [self.knee],
2: [self.hip_pitch],
}, )
# explicit servo_id's
self.port_knee = 0
self.port_hip_pitch = 1
# create a moteus.Controller instance for each servo
self.servos = {
servo_id: moteus.Controller(id=servo_id, transport=self.transport)
for servo_id in [1, 2] # number of motors, need to change manually
}
# commands for the motors, default set to mid values
self.commands = [
self.servos[self.knee].make_position(
position=math.nan,
velocity=0.5,
maximum_torque=2.0,
stop_position=(MAX_POS_KN - MIN_POS_KN) / 2,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
self.servos[self.hip_pitch].make_position(
position=math.nan,
velocity=0.5,
maximum_torque=2.0,
stop_position=(MAX_POS_HP - MIN_POS_HP) / 2,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
]
async def main():
# By default, Controller connects to id 1, and picks an arbitrary
# CAN-FD transport, prefering an attached fdcanusb if available.
c = moteus.Controller()
# In case the controller had faulted previously, at the start of
# this script we send the stop command in order to clear it.
await c.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 c.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)
import signal
import sys
import time
import asyncio
import moteus
c = moteus.Controller()
running = True
def signal_handler(signal, frame):
print('Stopping gradually ....')
global running
running = False
signal.signal(signal.SIGINT, signal_handler)
async def main():
i = 0
while running:
i = i + 1
# await c.set_position(position=None, velocity=None, feedforward_torque=0.1, watchdog_timeout=0.5)
if not (i % 10):
print(await c.query())
await asyncio.sleep(0.01)
async def main():
transport = moteus_pi3hat.Pi3HatRouter(servo_bus_map={
1: [13, 23, 24],
2: [14, 18, 17],
3: [16, 22, 21],
4: [15, 19, 20]
})
c13 = moteus.Controller(id=13, transport=transport)
c14 = moteus.Controller(id=14, transport=transport)
c15 = moteus.Controller(id=15, transport=transport)
c16 = moteus.Controller(id=16, transport=transport)
c17 = moteus.Controller(id=17, transport=transport)
c18 = moteus.Controller(id=18, transport=transport)
c19 = moteus.Controller(id=19, transport=transport)
c20 = moteus.Controller(id=20, transport=transport)
c21 = moteus.Controller(id=21, transport=transport)
c22 = moteus.Controller(id=22, transport=transport)
c23 = moteus.Controller(id=23, transport=transport)
c24 = moteus.Controller(id=24, transport=transport)
await c13.set_stop()
await c14.set_stop()
await c15.set_stop()
await c16.set_stop()
await c17.set_stop()
await c18.set_stop()
await c19.set_stop()
await c20.set_stop()
await c21.set_stop()
await c22.set_stop()
await c23.set_stop()
await c24.set_stop()
maxT = 0.1
while True:
await transport.cycle([
c13.make_position(position=0.3499, maximum_torque=maxT,
query=True),
c23.make_position(position=0.15617 - 1,
maximum_torque=maxT,
query=True),
c24.make_position(position=0.1710, maximum_torque=maxT,
query=True),
c14.make_position(position=0.74639,
maximum_torque=maxT,
query=True),
c17.make_position(position=0.32684,
maximum_torque=maxT,
query=True),
c18.make_position(position=-0.73236 + 1,
maximum_torque=maxT,
query=True),
c16.make_position(position=-0.38513,
maximum_torque=maxT,
query=True),
c21.make_position(position=0.50958,
maximum_torque=maxT,
query=True),
c22.make_position(position=0.328186,
maximum_torque=maxT,
query=True),
c15.make_position(position=-0.920715,
maximum_torque=maxT,
query=True),
c19.make_position(position=0.6815185 - 1,
maximum_torque=maxT,
query=True),
c20.make_position(position=1.155334 - 1,
maximum_torque=maxT,
query=True),
])
if maxT < 0.4:
maxT = maxT + 0.05
await asyncio.sleep(0.05)
async def main():
print("creating moteus pi3hat transport")
# our system has 2 servos, each attached to a separate pi3hat bus
# servo_bus_map argument describes which IDs are found on which bus
transport = moteus_pi3hat.Pi3HatRouter(
servo_bus_map={
1: [1], # KNEE
2: [2], # HIP
}, )
# We create one 'moteus.Controller' instance for each servo. It is not
# strictly required to pass a 'transport' since we do not intend to use
# any 'set_*' methods, but it doesn't hurt.
#
# syntax is a python "dictionary comprehension"
servos = {
servo_id: moteus.Controller(id=servo_id, transport=transport)
for servo_id in [1, 2]
}
# We will start by sending a 'stop' to all servos, in the event that any had a fault
await transport.cycle([x.make_stop() for x in servos.values()])
print("sent stop cmd to clear any motor faults")
while True:
# the 'cycle' method accepts a list of commands, each of which is created by
# calling one of the 'make_foo' methods on Controller. The most command thing
# will be the 'make_position' method
now = time.time()
# construct a pos command for each servo, each of which consists of a
# sinusoidal velocity command starting from wherever the servo was at
# to begin with
#
# 'make_position' accepts optional keyword arguments that correspond
# to each of the available position mode registers in the moteus
# reference manual
half_kn = (MAX_POS_KN - MIN_POS_KN) / 2
half_hp = (MAX_POS_HP - MIN_POS_HP) / 2
commands_sinusoidal = [
servos[1].make_position( # KNEE
position=math.nan,
velocity=0.5,
maximum_torque=2.0,
stop_position=MIN_POS_KN + half_kn + math.sin(now) * half_kn,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
servos[2].make_position( # HIP
position=math.nan,
velocity=0.5,
maximum_torque=2.0,
stop_position=MIN_POS_HP + half_hp +
math.sin(now + 1) * half_hp,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
]
# position commands
commands_pos = [
servos[1].make_position( # KNEE
position=math.nan,
velocity=2.0,
maximum_torque=2.0,
stop_position=MAX_POS_KN,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
servos[2].make_position( # HIP
position=math.nan,
velocity=2.0,
maximum_torque=2.0,
stop_position=MAX_POS_HP,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
]
# By sending all commands to the transport in one go, the pi3hat
# can send out commands and retrieve responses simultaneously
# from all ports. It can also pipeline commands and responses
# for multiple servos on the same bus
results = await transport.cycle(commands_sinusoidal)
#results = await transport.cycle(commands_pos)
# The result is a list of 'moteus.Result' types, each of which
# identifies the servo it came from, and has a 'values' field
# that allows access to individual register results.
#
# NOTE: it is possible to not receive responses from all servos
# for which a query was requested
#
# Here, we'll just print the ID, position, and velocity of each
# servo for which a reply was returned
print("\n".join(f"({result.id}) " +
f"{result.values[moteus.Register.POSITION]} " +
f"{result.values[moteus.Register.VELOCITY]} "
for result in results),
end='\r')
#print(now, end='\r')
# We will wait 20ms between cycles. By default, each servo has
# a watchdog timeout, where if no CAN command is received for
# 100mc the controller will enter a latched fault state
await asyncio.sleep(0.02)
def __init__(self, transport, id):
self.id = id
self.transport = transport
self.controller = moteus.Controller(id=id, transport=transport)
self.stream = moteus.Stream(self.controller)
async def main():
# The parameters of each CAN bus can be set at construction time.
# The available fields can be found in the C++ header at
# Pi3Hat::CanConfiguration
slow_can = moteus_pi3hat.CanConfiguration()
slow_can.slow_bitrate = 125000
slow_can.fdcan_frame = False
slow_can.bitrate_switch = False
# If buses are not listed, then they default to the parameters
# necessary to communicate with a moteus controller.
can_config = {
5: slow_can,
}
transport = moteus_pi3hat.Pi3HatRouter(can=can_config)
# Since we didn't specify a 'servo_bus_map' to Pi3HatRouter, this
# will be assumed to be on bus 1.
controller = moteus.Controller(id=1, transport=transport)
while True:
# To send a raw CAN, you must manually instantiate a
# 'moteus.Command' and fill in its fields, along with which
# bus to send it on.
raw_message = moteus.Command()
raw_message.raw = True
raw_message.arbitration_id = 0x0405
raw_message.bus = 5
raw_message.data = b'1234'
raw_message.reply_required = False
# A single 'transport.cycle' call's message list can contain a
# mix of "raw" frames and those generated from
# 'moteus.Controller'.
#
# If you want to listen on a CAN bus without having sent a
# command with 'reply_required' set, you can use the
# 'force_can_check' optional parameter. It is a 1-indexed
# bitfield listing which additional CAN buses should be
# listened to.
results = await transport.cycle([
raw_message,
controller.make_query(),
],
force_can_check=(1 << 5))
# If any raw CAN frames are present, the result list will be a
# mix of moteus.Result elements and can.Message elements.
# They each have the 'bus', 'arbitration_id', and 'data'
# fields.
#
# moteus.Result elements additionally have an 'id' field which
# is the moteus servo ID and a 'values' field which reports
# the decoded response.
for result in results:
if hasattr(result, 'id'):
# This is a moteus structure.
print(f"{time.time():.3f} MOTEUS {result}")
else:
# This is a raw structure.
print(
f"{time.time():.3f} BUS {result.bus} " +
f"ID {result.arbitration_id:x} DATA {result.data.hex()}")
await asyncio.sleep(1.0)
async def main():
# We will be assuming a system where there are 4 servos, each
# attached to a separate pi3hat bus. The servo_bus_map argument
# describes which IDs are found on which bus.
transport = moteus_pi3hat.Pi3HatRouter(
servo_bus_map = {
1:[11],
2:[12],
3:[13],
4:[14],
},
)
# We create one 'moteus.Controller' instance for each servo. It
# is not strictly required to pass a 'transport' since we do not
# intend to use any 'set_*' methods, but it doesn't hurt.
#
# This syntax is a python "dictionary comprehension":
# https://docs.python.org/3/tutorial/datastructures.html#dictionaries
servos = {
servo_id : moteus.Controller(id=servo_id, transport=transport)
for servo_id in [11, 12, 13, 14]
}
# We will start by sending a 'stop' to all servos, in the event
# that any had a fault.
await transport.cycle([x.make_stop() for x in servos.values()])
while True:
# The 'cycle' method accepts a list of commands, each of which
# is created by calling one of the `make_foo` methods on
# Controller. The most common thing will be the
# `make_position` method.
now = time.time()
# For now, we will just construct a position command for each
# of the 4 servos, each of which consists of a sinusoidal
# velocity command starting from wherever the servo was at to
# begin with.
#
# 'make_position' accepts optional keyword arguments that
# correspond to each of the available position mode registers
# in the moteus reference manual.
commands = [
servos[11].make_position(
position=math.nan,
velocity=0.1*math.sin(now),
query=True),
servos[12].make_position(
position=math.nan,
velocity=0.1*math.sin(now + 1),
query=True),
servos[13].make_position(
position=math.nan,
velocity=0.1*math.sin(now + 2),
query=True),
servos[14].make_position(
position=math.nan,
velocity=0.1*math.sin(now + 3),
query=True),
]
# By sending all commands to the transport in one go, the
# pi3hat can send out commands and retrieve responses
# simultaneously from all ports. It can also pipeline
# commands and responses for multiple servos on the same bus.
results = await transport.cycle(commands)
# The result is a list of 'moteus.Result' types, each of which
# identifies the servo it came from, and has a 'values' field
# that allows access to individual register results.
#
# Note: It is possible to not receive responses from all
# servos for which a query was requested.
#
# Here, we'll just print the ID, position, and velocity of
# each servo for which a reply was returned.
print(", ".join(
f"({result.id} " +
f"{result.values[moteus.Register.POSITION]} " +
f"{result.values[moteus.Register.VELOCITY]})"
for result in results))
# We will wait 20ms between cycles. By default, each servo
# has a watchdog timeout, where if no CAN command is received
# for 100ms the controller will enter a latched fault state.
await asyncio.sleep(0.02)
def __init__(self, id):
self.id = id
self.controller = moteus.Controller(id = id)
self.stream = moteus.Stream(self.controller) # moteus diagnostic stream, do NOT use for real time
import sys
# TODO: check out and if needed tune/change the motor's internal PID
# in order to increase damping!
knee = 1
hip = 2
transport = moteus_pi3hat.Pi3HatRouter(
servo_bus_map={
1: [knee], # KNEE
2: [hip], # HIP
}, )
servos = {
servo_id: moteus.Controller(id=servo_id, transport=transport)
for servo_id in [1, 2]
}
# NOTE: sin controller: Kp, dt, l0, l1, animation)
ctrlr = sinIkHopCtrlr(25.0, 0.015, 0.1, 0.15, False)
ctrlr_x = ctrlr_y = 0
theta0 = 0 # hip angle
theta1 = 0 # knee angle
commands = []
ctrlr.q[0] = 0 # x ft pos in sim
ctrlr.q[1] = 0 # y ft pos in sim
async def main():
import signal
import sys
import time
import asyncio
import numpy as np
import board
import busio
import adafruit_bno055
import moteus
c1 = moteus.Controller()
c2 = moteus.Controller(id=2)
i2c = busio.I2C(board.SCL, board.SDA)
sensor = adafruit_bno055.BNO055_I2C(i2c)
sensor.mode = adafruit_bno055.IMUPLUS_MODE
running = True
def signal_handler(signal, frame):
print('Stopping gradually ....')
global running
running = False
signal.signal(signal.SIGINT, signal_handler)
def __init__(self, id):
self.id = id
self.controller = moteus.Controller(id = id)
self.stream = moteus.Stream(self.controller)
