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)