#!/usr/bin/python3
from pyPMX import *
from pyDXL import *
import math, kbhit, time, serial, threading

# Communication task with PMX
def func1(arg):
  id = 0
  # Class instantiation
  pmx = PMXProtocol(arg[0], lock = arg[1])
  # Setting initial conditions
  pmx.MemREAD(id, 400, 6)      # Clear error flag
  pmx.MotorWRITE(id, 2, ())    # Torque free
  pmx.MemWRITE8(id, 501, (1, 0b11111))  # Control mode (All)
  pmx.MotorWRITE(id, 1, ())    # Torque enable
  while arg[2]:
    ang = int(math.sin(time.time()) * 9000.0)
    r = pmx.MotorWRITE(id, 0, (ang,)) # Angle command
    if r != None:
      arg[3] = r[1][0]
    time.sleep(0.001)
  pmx.MotorWRITE(id, 2, ())    # Torque free
  del pmx

# Communication task with DXL
def func2(arg):
  id = 1
  cnt = 0
  led = 0
  # Class instantiation
  dx2 = DXLProtocolV2(arg[0], lock = arg[1])
  # Setting initial conditions
  dx2.Write8(id, 64, 1)        # Torque enable
  while arg[2]:
    ang = int(math.sin(time.time()) * 1024.0) + 2047
    dx2.Write32(id, 116, ang)  # Angle command
    r = dx2.Read32(id, 132, signed = True)  # Read feedback values
    if r != None:
      arg[4] = r
    cnt += 1
    if cnt % 10 == 0:
      led ^= 1
      dx2.Write8(id, 65, led)  # Update LED status
    time.sleep(0.001)
  dx2.Write8(id, 65, 0)        # LED off
  dx2.Write8(id, 64, 0)        # Torque free
  del dx2

kb = kbhit.KBHit()
# Open serial port
com = serial.Serial('/dev/ttyAMA0', 1000000, timeout=0.005)
lock = threading.Lock()
act = True
v1 = 0
v2 = 0
SharedVar = [com, lock, act, v1, v2]

# Start thread
th1 = threading.Thread(target = func1, args = [SharedVar])
th2 = threading.Thread(target = func2, args = [SharedVar])
th1.start()
th2.start()

# Loop until keyboard input
while True:
  if kb.kbhit():
    break
  print(f' pmx:{SharedVar[3]:6d}, dxl:{SharedVar[4]:6d}', end = '\r')
  time.sleep(0.05)

# End
SharedVar[2] = False
th1.join()
th2.join()

del kb, lock, com, th1, th2
print()

#!/usr/bin/python3
import time
from pyPMX import PMXProtocol

ID = 0
BAUD = 3000000
# Create pmx instance
pmx = PMXProtocol('/dev/ttyAMA0', BAUD)

pmx.MemREAD(ID, 400, 6) # Error clear
pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ()) # Torque off
pmx.MemWRITE8(ID, 501, 1)  # bit 0:pos,1:speed,2:cur,3:torq,4:pwm,5:time
pmx.MotorWRITE(ID, pmx.MOTW_OPT_TORQUEON, ()) # Torque on

for ang in (0, 4500, 9000, 4500, 0, -4500, -9000, -4500, 0):
  pmx.MotorWRITE(ID, pmx.MOTW_OPT_NONE, (ang,)) # Command angle
  time.sleep(0.5)
pmx.MotorWRITE(ID, pmx.MOTW_OPT_FREE, ()) # Torque off

#!/usr/bin/python3

_retrynum = 3
# mode set & torque enable
def _setmode(pmx, id, m):
  for i in range(_retrynum):
    if pmx.MemWRITE8(id, 500, pmx.MOTW_OPT_FREE):
      if m != 0:
        # 1:pos,2:speed,4:cur,8:torq,16:pwm,32:time
        if pmx.MemWRITE8(id, 501, m):
          if pmx.MemWRITE8(id, 502, 0b11111):
            if pmx.MemWRITE8(id, 503, 5):
              if pmx.MemWRITE8(id, 500, pmx.MOTW_OPT_TORQUEON):
                return True
      else:
        return True
  return False

def SetMode(pmx, ids, m):
  if isinstance(ids, list) or isinstance(ids, tuple):
    r = 0
    for i, id in enumerate(ids):
      if isinstance(m, list) or isinstance(m, tuple):
        r += 1 if _setmode(pmx, id, m[i]) else 0
      else:
        r += 1 if _setmode(pmx, id, m) else 0
    return True if r == len(ids) else False
  else:
    return _setmode(pmx, ids, m)

# set angle
def _setangle(pmx, id, ang, t):
  for i in range(_retrynum):
    r = pmx.MotorWRITE(id, pmx.MOTW_OPT_NONE, (int(ang * 100), t))
    if r != None:
      if len(r) > 0:
        return r[1]
  return ()

def SetAngle(pmx, ids, angs, t):
  if isinstance(ids, list) or isinstance(ids, tuple):
    r = ()
    for i, id in enumerate(ids):
      if isinstance(angs, list) or isinstance(angs, tuple):
        r += _setangle(pmx, id, angs[i], t),
      else:
        r += _setangle(pmx, id, angs, t),
    return r
  else:
    return _setangle(pmx, ids, angs, t)

# set velocity
def _setvelocity(pmx, id, velo):
  for i in range(_retrynum):
    r = pmx.MotorWRITE(id, pmx.MOTW_OPT_NONE, (int(velo),))
    if r != None:
      if len(r) > 0:
        return r[1]
  return ()

def SetVelocity(pmx, ids, velos):
  if isinstance(ids, list) or isinstance(ids, tuple):
    r = ()
    for i, id in enumerate(ids):
      if isinstance(velos, list) or isinstance(velos, tuple):
        r += _setvelocity(pmx, id, velos[i]),
      else:
        r += _setvelocity(pmx, id, velos),
    return r
  else:
    return _setvelo(pmx, ids, velos)

def _getpresent(pmx, id):
  for i in range(_retrynum):
    r = pmx.MemREAD16(id, 300, 5, signed = True)
    if r != None:
      return r
  return ()

def GetPresent(pmx, ids):
  if isinstance(ids, list) or isinstance(ids, tuple):
    r = ()
    for i, id in enumerate(ids):
      r += _getpresent(pmx, id),
    return r
  else:
    return _getpresent(pmx, ids)

if __name__ == "__main__":
  import os, sys, time, kbhit
  from pyPMX import PMXProtocol

  kb = kbhit.KBHit()
  pmx = PMXProtocol('/dev/ttyAMA0', 3000000, 0.005)

  ID1 = 0
  ID2 = 1

  # Position Control
  print('Position Control')
  print(f'{ID1}:angle velo   cur   torque  pwm   {ID2}:angle velo   cur   torque  pwm')
  if SetMode(pmx, (ID1, ID2), 1|32):
    while not kb.kbhit():
      t = 1000
      for ang in (0, 45, 90, 45, 0, -45, -90, -45):
        if kb.kbhit(): break
        SetAngle(pmx, (ID1, ID2), (ang, ang * 2), t)
        s = time.time() + t / 1000.0
        while s > time.time():
          if kb.kbhit(): break
          r = GetPresent(pmx, (ID1, ID2))
          for r in r:
            print(('{:6d} '*len(r)).format(*r), end='')
          print(end='\r')
    SetMode(pmx, (ID1, ID2), 0)
    kb.getch()

  # Velocity Control
  print('\nVelocity Control')
  print(f'{ID1}:angle velo   cur   torque  pwm   {ID2}:angle velo   cur   torque  pwm')
  if SetMode(pmx, (ID1, ID2), 2):
    while not kb.kbhit():
      t = 3000
      for velo in (0, 500, 1000, 1500, 1000, 500, 0, -500, -1000, -1500, -1000, -500):
        if kb.kbhit(): break
        SetVelocity(pmx, (ID1, ID2), (velo*1, velo*1.5))
        s = time.time() + t / 1000.0
        while s > time.time():
          if kb.kbhit(): break
          r = GetPresent(pmx, (ID1, ID2))
          for r in r:
            print(('{:6d} '*len(r)).format(*r), end='')
          print(end='\r')
    SetMode(pmx, (ID1, ID2), 0)

  del kb, pmx

Pythonその2

頒布版PicoSHIELDについて

dxlib1.5beta_for_arduino.zip

暗に新製品を想定している事を匂わせています