def connect(self):
if self.cmd is not None:
return
self.check_config()
cfg = self.config()
logger.info(
"TapeNode[%s] creating serial %s, %s",
self, cfg['loc'], cfg['baud'])
# add options for fake tape node
if cfg['loc'] == 'fake':
# make fake cmd and fake mgr
self.cmd = 'fake'
self.mgr = FakeManager(
commands, cfg['tension_target'], cfg['tension_range'])
self._barcodes = cfg.get(
'fakebarcodes', {}).get('initial', [])
# save initial barcodes
self._save_barcodes()
else:
self.serial = serial.Serial(cfg['loc'], cfg['baud'])
self.comando = pycomando.Comando(self.serial)
self.cmd = pycomando.protocols.CommandProtocol(self.comando)
self.comando.register_protocol(1, self.cmd)
self.mgr = pycomando.protocols.command.EventManager(
self.cmd, commands)
time.sleep(4) # wait for the arduino to start
self.mgr.on('error', self._error)
self._set_tension_limits()
#self.guess_state()
logger.info("TapeNode[%s] connected to %s", self, cfg['loc'])
def connect(port=None, baud=115200):
global conn, com, mgr
conn = serial.Serial(find_port(), baud)
com = pycomando.Comando(conn)
text = pycomando.protocols.TextProtocol(com)
cmd = pycomando.protocols.CommandProtocol(com)
com.register_protocol(0, text)
com.register_protocol(1, cmd)
mgr = pycomando.protocols.command.EventManager(cmd, commands)
return mgr
def __init__(self, port):
self.port = port
self._serial = serial.Serial(self.port, 9600)
# set rising edge of RTS to reset comando
self._serial.setRTS(0)
self._serial.flushInput()
self._serial.flushOutput()
self._serial.setRTS(1)
time.sleep(0.1)
self.com = pycomando.Comando(self._serial)
self.cmd = pycomando.protocols.command.CommandProtocol()
self.com.register_protocol(0, self.cmd)
mgr = pycomando.protocols.command.EventManager(self.cmd, base_cmds)
self._text = pycomando.protocols.text.TextProtocol()
self.name = names[mgr.blocking_trigger('name')[0].value]
super(TeensyBody, self).__init__(self.name)
self._last_hb = time.time()
mgr.trigger('heartbeat')
# clear callbacks, register name specific commands
self.cmd.callbacks = {}
self.mgr = pycomando.protocols.command.EventManager(
self.cmd, cmds[self.name])
def print_text(txt, n=self.name):
print("DEBUG[body.%s]:%s" % (n, txt))
self._text.register_callback(print_text)
self.com.register_protocol(1, self._text)
# setup report callbacks
def make_callback(n):
cbn = n
def cb(*args):
vs = [a.value for a in args]
#print("%s: %s" % (cbn, vs))
if len(vs) == 1:
self.log.debug({cbn: vs[0]})
else:
self.log.debug({cbn: vs})
self.trigger(cbn, *vs)
return cb
r = reports.get(self.name, {})
for k in r:
self.mgr.on(k, make_callback(k))
# setup reporting periods
self.mgr.trigger(k, r[k])
self.mgr.on('heading', self._on_heading)
self.mgr.trigger('heading', 500)
def __init__(self, port=None):
super(SteelJoystick, self).__init__()
self.mapping = default_mapping
if port is None:
ts = utils.find_teensies_by_type('joystick')
if len(ts) != 1:
raise IOError("Didn't find custom joystick: %s" % (ts, ))
port = ts[0]['port']
self.port = port
self.com = pycomando.Comando(serial.Serial(self.port, 9600))
self.cmd = pycomando.protocols.command.CommandProtocol()
self.com.register_protocol(0, self.cmd)
#self.text = pycomando.protocols.TextProtocol()
self.mgr = pycomando.protocols.command.EventManager(self.cmd, cmds)
# attach callbacks
self.mgr.on('axis', self.on_axis)
self.mgr.on('button', self.on_button)
def __init__(self, port):
print("Connecting to teensy on port: %s" % port)
self.port = port
#self._serial = serial.Serial(self.port, 9600)
self._serial = open_port(self.port)
# set rising edge of RTS to reset comando
self._serial.setRTS(0)
self._serial.flushInput()
self._serial.flushOutput()
self._serial.setRTS(1)
#time.sleep(0.5)
# start up comando
self.com = pycomando.Comando(self._serial)
self.cmd = pycomando.protocols.command.CommandProtocol()
self._text = pycomando.protocols.text.TextProtocol()
self.com.register_protocol(0, self.cmd)
# used for callbacks
self.mgr = pycomando.protocols.command.EventManager(self.cmd, cmds)
# easier for calling
# self.ns = self.mgr.build_namespace()
# get leg number
logger.debug("%s Get leg number" % port)
ln = self.mgr.blocking_trigger('leg_number')[0].value
print("Connected to leg %s on port %s" % (ln, port))
super(Teensy, self).__init__(ln)
def print_text(txt, leg_number=ln):
print("DEBUG[%s]:%s" % (leg_number, txt))
self._text.register_callback(print_text)
self.com.register_protocol(1, self._text)
# load calibration setup
for v in calibration.setup.get(self.leg_number, []):
self.log.debug({'calibration': v})
print("calibration: %s" % v)
f, args = v
logger.debug("Calibration: %s, %s" % (f, args))
self.mgr.trigger(f, *args)
# write out leg geometry
for (ji, jn) in enumerate(['hip', 'thigh', 'knee']):
j = getattr(self.geometry, jn)
for attr in consts.GEOM_INDEX_BY_NAME:
code = consts.GEOM_INDEX_BY_NAME[attr]
value = getattr(j, attr)
self.log.debug({'set_geometry': (ji, code, value)})
self.mgr.trigger('set_geometry', ji, code, value)
self.mgr.on('estop', self.on_estop)
self.loop_time_stats = utils.StatsMonitor()
# disable leg
self.set_estop(consts.ESTOP_DEFAULT)
# verify seed time against python code
seed_time = self.mgr.blocking_trigger('pid_seed_time')[0].value
# set plan tick on first leg connected
if consts.PLAN_TICK is None:
# round to nearest ms
consts.PLAN_TICK = numpy.round(seed_time * 1000.) / 1000.
if abs(seed_time - consts.PLAN_TICK) > 1E-9:
raise ValueError(
"PID seed time [%s] for leg %s does not match python %s" %
(seed_time, self.leg_number, consts.PLAN_TICK))
# send first heartbeat
self.send_heartbeat()
self.mgr.on('report_xyz', self.on_report_xyz)
self.mgr.on('report_angles', self.on_report_angles)
self.mgr.on('report_pid', self.on_report_pid)
self.mgr.on('report_pwm', self.on_report_pwm)
self.mgr.on('report_adc', self.on_report_adc)
self.mgr.on('report_loop_time', self.on_report_loop_time)
self.calibrators = {
'calf': calibration.CalfCalibrator(),
# hip, thigh, knee
}
# request current calibration values
self.calibrators['calf'].attach_manager(self.mgr)
#!/usr/bin/env python
import time
import serial
import pycomando
com = pycomando.Comando(serial.Serial('/dev/ttyACM0', 9600))
text = pycomando.protocols.TextProtocol(com)
com.register_protocol(0, text)
def show(text):
print(text)
text.register_callback(show)
while True:
com.handle_stream()
time.sleep(0.01)
# ctypes is useful for defining fixed size data types
import ctypes
import sys
import pycomando
import serial
if len(sys.argv) < 2:
raise Exception("A serial port must be supplied: commands.py <port>")
port = sys.argv[1]
# open the serial port
serial_port = serial.Serial(port, 9600)
# create comando, the stream handler
com = pycomando.Comando(serial_port)
# create a few protocols to receive text and send/receive commands
text = pycomando.protocols.TextProtocol(com)
cmd = pycomando.protocols.CommandProtocol(com)
# register the created protocols, the protocol ids (first argument)
# should match on both the arduino and in python
com.register_protocol(0, text)
com.register_protocol(1, cmd)
# define a callback to be used for the text protocol
def print_message(msg):
# just print out the message with a header
print("from arduino->%s" % msg)
def get_time():
global teensy_time, receipt_time
teensy_time = None
receipt_time = None
sent_time = time.time()
cmd.send_command(5)
while teensy_time is None:
com.handle_stream()
return teensy_time, sent_time, receipt_time
# connect to teensy
conn = serial.Serial('/dev/ttyACM0', 115200)
com = pycomando.Comando(conn)
text = pycomando.protocols.TextProtocol(com)
cmd = pycomando.protocols.CommandProtocol(com)
com.register_protocol(0, text)
com.register_protocol(1, cmd)
cmd.register_callback(5, new_time)
# sync
t1p, t1, _ = get_time()
# delay_request
t2, _, t2p = get_time()
# compute offset
offset = (t1p - t1 - t2p + t2) / 2.
print("Times:")
print("\tt1p: %s" % t1p)
# play with the path so we can import pycomando even if it's nto installed
try:
import pycomando
except ImportError:
sys.path.append('../../')
import pycomando
# connect to the serial port
port = serial.Serial('/dev/ttyACM0', 9600)
# if this is an arduino, reset it and wait
#time.sleep(1) # wait for arduino
#port.setDTR(level=0)
#time.sleep(1)
# create our stream handler
com = pycomando.Comando(port)
# make a very simple text protocol
text = pycomando.protocols.TextProtocol(com)
# define a default message callback
# this will get called whenever a message is received
def show(bs):
print("Arduino said: %s" % bs)
# tell the text protocol what to do when a message is received
text.receive_message = show
# register the text protocol with comando
com.register_protocol(0, text)
'result': (ctypes.c_int32, ),
},
31: {
'name': 'set_tension_limits',
'args': (ctypes.c_int32, ctypes.c_int32),
},
32: {
'name': 'get_tension_limits',
'result': (ctypes.c_int32, ctypes.c_int32),
},
}
l = smaract.find_systems()[0]
m = smaract.MCS(l)
c = pycomando.Comando(serial.Serial(port, baud))
echo = pycomando.protocols.EchoProtocol(c)
cmd = pycomando.protocols.CommandProtocol(c)
c.register_protocol(0, echo)
c.register_protocol(1, cmd)
mgr = pycomando.protocols.command.EventManager(cmd, commands)
def throw_error(code):
raise Exception(code)
mgr.on('error', throw_error)
def __init__(self, port):
self.port = port
self.com = pycomando.Comando(serial.Serial(self.port, 9600))
self.cmd = pycomando.protocols.command.CommandProtocol()
self.text = pycomando.protocols.TextProtocol()
self.com.register_protocol(0, self.cmd)
self.com.register_protocol(1, self.text)
self.text.receive_message = print_message
# used for callbacks
self.mgr = pycomando.protocols.command.EventManager(self.cmd, cmds)
# easier for calling
# self.ns = self.mgr.build_namespace()
# get leg number
logger.debug("%s Get leg number" % port)
ln = self.mgr.blocking_trigger('leg_number')[0].value
super(Teensy, self).__init__(ln)
self._text = pycomando.protocols.text.TextProtocol()
def print_text(txt, leg_number=ln):
print("DEBUG[%s]:%s" % (leg_number, txt))
self._text.register_callback(print_text)
self.com.register_protocol(1, self._text)
# load calibration setup
for v in calibration.setup.get(self.leg_number, []):
self.log.debug({'calibration': v})
f, args = v
logger.debug("Calibration: %s, %s" % (f, args))
self.mgr.trigger(f, *args)
self.mgr.on('estop', self.on_estop)
self.loop_time_stats = utils.StatsMonitor()
# disable leg
self.set_estop(consts.ESTOP_DEFAULT)
# verify seed time against python code
seed_time = self.mgr.blocking_trigger('pid_seed_time')[0].value
# set plan tick on first leg connected
if consts.PLAN_TICK is None:
# round to nearest ms
consts.PLAN_TICK = numpy.round(seed_time * 1000.) / 1000.
if abs(seed_time - consts.PLAN_TICK) > 1E-9:
raise ValueError(
"PID seed time [%s] for leg %s does not match python %s" %
(seed_time, self.leg_number, consts.PLAN_TICK))
# send first heartbeat
self.send_heartbeat()
self.mgr.on('report_xyz', self.on_report_xyz)
self.mgr.on('report_angles', self.on_report_angles)
self.mgr.on('report_pid', self.on_report_pid)
self.mgr.on('report_pwm', self.on_report_pwm)
self.mgr.on('report_adc', self.on_report_adc)
self.mgr.on('report_loop_time', self.on_report_loop_time)
self.calibrators = {
'calf': calibration.CalfCalibrator(),
# hip, thigh, knee
}
# request current calibration values
self.calibrators['calf'].attach_manager(self.mgr)
#!/usr/bin/env python3
import sys
import time
import serial
import pycomando
import stompy
if len(sys.argv) < 1:
raise Exception("Must provide port as argument")
if len(sys.argv) < 2:
raise Exception("Must provide leg number as second argument")
port = sys.argv[1]
leg_number = int(sys.argv[2])
comm = serial.Serial(port, 9600)
com = pycomando.Comando(comm)
cmd = pycomando.protocols.command.CommandProtocol()
com.register_protocol(0, cmd)
mgr = pycomando.protocols.command.EventManager(cmd, stompy.leg.teensy.cmds)
r = mgr.blocking_trigger('leg_number', leg_number)
time.sleep(0.1)
r = mgr.blocking_trigger('leg_number')[0].value
print("Leg number is now: %s" % r)
print("Don't forget to add serial number to stompy/utils.py")