Source

puma / mdc-telnet.py

Trammell Hudson 08ec878 
Trammell Hudson a6c5766 




Trammell Hudson 08ec878 



Trammell Hudson a6c5766 
Trammell Hudson 08ec878 


Trammell Hudson a6c5766 
Trammell Hudson 08ec878 

Trammell Hudson 5ffbf16 












Trammell Hudson a6c5766 
Trammell Hudson 08ec878 

Trammell Hudson a6c5766 
Trammell Hudson e4c0d32 










Trammell Hudson b3bce6e 





Trammell Hudson a6c5766 







Trammell Hudson 08ec878 
Trammell Hudson a6c5766 




Trammell Hudson b3bce6e 







Trammell Hudson aef2c9d 

Trammell Hudson b3bce6e 

Trammell Hudson 5ffbf16 
Trammell Hudson a6c5766 
Trammell Hudson 6a3ec93 


Trammell Hudson e4c0d32 
Trammell Hudson 08ec878 
Trammell Hudson 0e58d93 




Trammell Hudson 08ec878 
Trammell Hudson 874e48b 


Trammell Hudson 08ec878 
Trammell Hudson 874e48b 
Trammell Hudson 0e58d93 
Trammell Hudson 08ec878 
#!/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

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])
		dest = [int(x) for x in args[2:]]
		MDC.move_all(ms, dest)
		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)

class ServerThread(SocketServer.ThreadingMixIn, SocketServer.TCPServer):
	# Force SOREUSEADDR allways
	daemon_threads = True
	allow_reuse_address = True

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 = ServerThread((host,port), MDCSocket)
	server.serve_forever()