puma / mdc-telnet.py

#!/usr/bin/python
# Telnet interface to the robot controllers
#
# Reports positions, velocities and amperages periodically to all clients.
# Accepts commands for joint position and speeds.
#
import SocketServer
import MDC
import threading
import time
import re

# Force SOREUSEADDR allways
SocketServer.TCPServer.allow_reuse_address = True

class MDCSocket(SocketServer.StreamRequestHandler):
	def handle(self):
		self.re = re.compile('^([0-9]),(-?[0-9]+)(?:,([0-9]+))?$')
		self.alive = True
		self.timeout = 1
		self.thread = threading.Thread(target = self.output_periodic)
		self.thread.daemon = True
		self.thread.start()

		while True:
			line = self.rfile.readline().strip()
			if not line:
				break
			print "Read line '" + line + "'"
			m = self.re.match(line)
			if m is None:
				self.wfile.write("! parse error\n")
				continue
			axis = int(m.group(1))
			pos = int(m.group(2))
			vel = m.group(3)
			if vel is not None:
				vel = int(vel)

			if not MDC.moveto(axis, pos, vel):
				self.wfile.write("! bad axis " + str(axis) + "\n")

		self.alive = False
		print "Client exited"
	
	def output_periodic(self):
		while self.alive:
			self.wfile.write(','.join(MDC.get_counts()) + '\n')
			time.sleep(1)

if __name__ == "__main__":
	devices = ["/dev/tty.usbmodemfd1231", "/dev/tty.usbmodemfd1241", "/dev/tty.usbmodemfd1271"]
	controllers = [MDC.MDC(dev) for dev in devices]

	host = 'localhost'
	port = 31415
	server = SocketServer.TCPServer((host,port), MDCSocket)
	server.serve_forever()
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.