puma / mdc-telnet.py

# 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.rx_thread = threading.Thread(target = self.rx_loop)
		#self.rx_thread.daemon = True

		self.tx_thread = threading.Thread(target = self.tx_loop)
		self.tx_thread.daemon = True

		# why doesn't this work as a thread?

	def rx_loop(self):
		while self.alive:
			line = self.rfile.readline().strip()
			if not line:
			print "Read line '" + line + "'"

			if line == "save":
				counts = MDC.get_counts()
				for i in range(0,len(counts)):
					MDC.moveto(i+1, counts[i], 0)
			if line == "home":
				for i in range(0,6):
					MDC.moveto(i+1, 0, 500)

			m = self.re.match(line)
			if m is None:
				self.wfile.write("! parse error\n")
			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 tx_loop(self):
		while self.alive:
			#counts = MDC.get_volts()
			counts = MDC.get_counts()
			self.wfile.write(','.join(counts) + '\n')

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