Source

puma / mdc-telnet.py

Full commit
#!/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.alive = True
		self.timeout = 1

		#self.rx_thread = threading.Thread(target = self.rx_loop)
		#self.rx_thread.daemon = True
		#self.rx_thread.start()

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

		# why doesn't this work as a thread?
		self.rx_loop()

	def rx_loop(self):
		while self.alive:
			line = self.rfile.readline().strip()
			if not line:
				break
			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)
				continue
			if line == "home":
				for i in range(0,6):
					MDC.moveto(i+1, 0, 500)
				continue

			if line.startswith("go,"):
				if not self.go(line):
					self.wfile.write("! go error\n")
				continue

			m = re.match('^([0-9]),(-?[0-9]+)(?:,([0-9]+))?$', 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"

	# Parse a "go" command with the six joint positions:
	# go,ms,1,2,3,4,5,6
	def go(self,line):
		args = line.split(",")
		if len(args) != 8:
			return False
		ms = int(args[1])
		counts = MDC.get_counts()
		for i in range(0,6):
			if counts[i] == 'X':
				continue
			new_pos = int(args[i+2])
			delta = int(counts[i]) - new_pos
			if delta < 0:
				delta = -delta
			if ms != 0:
				MDC.moveto(i+1, new_pos, delta * 1000 / ms)
			else:
				MDC.moveto(i+1, new_pos, 0)
		return True

	def tx_loop(self):
		while self.alive:
			#counts = MDC.get_volts()
			counts = MDC.get_counts()
			self.wfile.write(','.join(counts) + '\n')
			time.sleep(0.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()