#!/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

ROSに興味があってもTurtleBotと直接的に関連する書籍が無いとお嘆きの貴兄。こんな書籍が発売されますので、ご参考にされてはいかがでしょう。

興味を持って頂いた方には昨年より評価を行って頂いているToF(Time of Flight )センサですが、ワンメイクで終わらせずに採番し製品化する事を検討しています。
何種類かのセンサを試した上で、最終的にST社のLV53L0Xを装備し、例の如くDynamixelと同等のTTLないしRS-485 I/Fを介してアクセスする仕様としました。センサとI/F間の仲介役としてDXMIOと同様にNXP LPC824を搭載し、センサの情報を咀嚼してI/Fに流すといった役割を担います。
また、PSDセンサなどからの置き換えを意識し、アナログ電圧出力とデジタル出力を備えます。

製品化前ですが、評価時のドキュメントを最終版を意識して記載内容が更新されていますので、興味のある方はこちらをご覧下さい。

他に、プロトコルV2.0に対応したDynamixelシリーズが増えてきましたが、USB-3WAYやDXHUBは3Mbpsを超えるボーレートに対応していません。PCにおいて4Mbpsや4.5Mbpsで通信できれば、それだけで時間を有意義に利用できる事から、対応を望む声が少なからずありました。
単純にUSBシリアル変換ICをFT232Hに変更するだけで高いボーレートに対応しますが、それだけでは芸が無いので、USB-3WAYの後継のみ仕様を確定しました。
・D-SUBコネクタからの変換が既にレガシーなのでJSTのコネクタを装備
・Dongleシリーズでは中途半端に絶縁機能が入っていのでバスパワーのみで動作
・TTLとRS-485両I/Fに対応し、最大12Mbpsをサポート(FT232Hの仕様上限)


実機はこんな感じです(左:ToFセンサ、右:USBシリアル変換)。


製品化されたあかつきにはぜひご利用ください。

今回は最終で、「Palmi ちょっとCommander」を紹介します。



「Palmiにちょっと何かをやらせてみよう」というアプリケーションだそうです。

画面を一瞬見た時に、「ん？　もしかしてむずかしい？」と思うかもしれませんが、
いえいえ、むずかしいことはなく、Palmiにやって欲しい動作を選んで組合わせていくだけなんです。

どこをどう設定したらよいのか、まず迷うと思うので、最初にここを覗いておくとよいですよ。




「ちょっとコマンダー」でPalmiに設定した動作を、Palmi Fwapparの「やくそくごと」で確認できます。



それでは試しに、TVコマーシャルの「たけし、バカヤロー」をPalmiにやってもらいましょう！　

ちょっとコマンダーには、CMモーションが用意されているんですよぉ。
では、順番に設定してみますね。

「ToDoのキッカケを設定する」の文字をクリックすると別画面が表示されるので、“いつ？”は“顔を見たら”にして、　“誰の？”は“誰でも”を選択してみました。



次に、下記のように設定します。※１．２．３は順不同でOK。
１．「動き」のリスト→“話す（モーション付き）”を選択。
２．画面右下の「どのモーションをしますか？」→“CMモーション”をクリックして選択。
３．「何を話しますか？」→“たけし、バカヤロー”の言葉を入力。
４．「ToDoのプレビュー」ボタンを押す→Palmiが“たけし、バカヤロー”を動作する。

では、この動作に名前を付けて保存をすることに。
１．画面左上の「ToDoの名前を入力してください」をクリック→“たけし、バカヤロー”を入力。
２．画面下の「ToDoの保存」ボタンを押して完了！



Palmi Fwapparの「やくそくごと」に、しっかりと登録されています。



そしてこの後、Palmiに実証してもらおうと私の顔をみてもらったのですが、
残念ながら、“たけし、バカヤロー”のモーションをしてくれませんでした・・・・。
何でだろうなぁ・・・・

では、“誰でも顔をみたら”ではなく、時間で設定しなおしてみたら、
設定した時間に、Palmiは、“たけし、バカヤロー”をやってくれました！

本当は、時間ではなくて、“誰でも顔をみたら”で動作してほしいなぁ・・・・。

Palmiがやってきた

試作中のLWIR Camの評価ついでに、「あついあつい」言ってた輩を観察。

背中あたりがなかなかホット。
どちらかと言えば末端冷え性。

首筋には襟巻き禁物ですね。クリスマスデコレーションの際は気をつけないと。

Palmiがやってきた