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")