import time
from pyDXL import *
dx = DXLProtocolV2('/dev/ttyAMA0', 57600, 0.05)
ID = 1
dx.Write8(ID, 512, 0)
if input('Clear? (y/N) ') == 'y':
  dx.TxPacket(ID, DXLProtocolV2.INST_CLEAR, (0x01, 0x44, 0x58, 0x4c, 0x22))
  dx.RxPacket(timeout = 5.0)
  time.sleep(1)
dx.Write8(ID, 33, 3)
dx.Write8(ID, 512, 1)
for i in range(-262144, 262144, 512):
  dx.Write32(ID, 532, i)
dx.Write8(ID, 512, 0)

#!/usr/bin/python3
#
# Link two servos with virtual wires

import os, platform, psutil, time, kbhit, serial, numpy as np
from PeriodicTimer import PeriodicTimer
from jp200class import packet

intervalms = 20

try:
  pr = psutil.Process(os.getpid())
  if platform.system() == 'Windows':
    pr.nice(psutil.HIGH_PRIORITY_CLASS)
  else:
    pr.nice(-10)
except:
  print('info:run with sudo')

# present value
present_dt = [('ang',float),('cur',float),('velo',float)]
present = np.empty(2, dtype = present_dt)
# calculated current command value
tcur = [0.0] * 2

#--------------------------------
# Periodically executed functioin
#--------------------------------
def cycfunc(*p):
  sv = p[0]
  acks = []
  # deviation from each other's position
  tcur = (
    np.clip((present[1]['ang'] - present[0]['ang']) * 0.15, -1000.0, 1000.0),
    np.clip((present[0]['ang'] - present[1]['ang']) * 0.15, -1000.0, 1000.0)
  )
  # send current command and current position acquisition to JP200
  sv.request(
    '#0EX=4SG2=9000;5500;10TC=%dTP=1000CACVCC'
    '#1EX=4SG2=9000;5500;10TC=%dTP=1000CACVCC'
    %(tcur[0], tcur[1]), acks
  )
  # analyze response
  if acks:
    for ack in acks:
      m = sv.p2.findall(ack)
      id = -1
      for n in m:
        try:
          if n[0] == '#' and int(n[1]) >= 0: id = int(n[1])
          elif n[0] == 'CA' and id >= 0: present[id]['ang'] = float(n[1])
          elif n[0] == 'CC' and id >= 0: present[id]['cur'] = float(n[1])
          elif n[0] == 'CV' and id >= 0: present[id]['velo'] = float(n[1])
        except:
          pass
      if id >= 0: print('[%d] %4d %8d %5d %5d  '%(id, tcur[id], present[id]['ang'], present[id]['velo'], present[id]['cur']), end = '')
    print(end = '\r')

# instantiate kbhit
kb = kbhit.KBHit()
# open UART
ser = serial.Serial('/dev/ttyAMA0', 2000000)
print(ser.name)
# instantiate JP200
sv = packet(ser, 0.02)
# instantiate periodic timer & start
tm = PeriodicTimer(intervalms / 1000, cycfunc, [sv])

acks = []
k = ''
while k != 'q':
  # waiting for key input
  try:
    k = kb.getch()
  except UnicodeDecodeError:
    pass
  print(k, end = '\n', flush = True)

  # move to origin position
  if k == 'h':
    tm.stop()
    sv.request('#0EX=1TA=18000TP=1000#1EX=1TA=18000TP=1000', acks)
  # stop timer
  elif k == 's':
    tm.stop()
    tm.start()
  # start timer
  elif k == 'e':
    tm.stop()
    sv.request('#0EX=0#1EX=0', acks)
  elif k == 'c':
    tm.stop()
    sv.request('#0EX=8TP=1000#1EX=8TP=1000', acks)
    time.sleep(5)
    sv.request('#0EX=8TP=-1000#1EX=8TP=-1000', acks)
    time.sleep(5)
    sv.request('#0EX=0#1EX=0', acks)
  else:
    print('XXXXXXXXX')
tm.stop()

sv.request('#0EX=0#1EX=0', acks, True)
ser.close()
kb.set_normal_term()
del sv, ser, kb

os._exit(0)