from hydraserial import HydraSerial
from sys import argv
from math import *
import struct
import time
ser = HydraSerial(argv[1], 115200)
def msg_recv(msg_id, payload):
if msg_id == 0x02:
print "theta: %f" % (degrees(struct.unpack("H", payload)[0] * (2.0 * pi) / 65536.0))
ser.write(0x01, struct.pack("<h", 2000 * sin(time.time())))
while True:
ser.read(msg_recv)
state[1] = state[1]%(2*pi)
motor.up = (sin(state[1]),0,cos(state[1]))
txq.put(state[1],False)
while not rxq.empty():
params[0]=rxq.get(False)
frameEnd = time.time()
sleepTime = max(dt-(frameEnd-frameStart),0)
time.sleep(sleepTime)
txq = Queue()
rxq = Queue()
p = Process(target=motorSim, args=(txq, rxq))
p.start();
ser = HydraSerial(argv[1],115200)
def msg_recv(msg_id, payload):
if msg_id == 0x01:
curr_dem = struct.unpack("h", payload)[0] * 0.01/32767.
txq.put(curr_dem)
print "demanded current: %f" % (curr_dem)
while True:
if not rxq.empty():
while not rxq.empty():
theta = rxq.get()
theta = struct.pack('<H', int(theta*65536./(2.*pi)))
ser.write(0x02, theta)
ser.read(msg_recv)
