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