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

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

			if line.startswith("move,"):
				if not self.move(line):
					self.wfile.write("! move 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

	# Parse a "move" command with the one joint number,
	# one position and one speed:
	# move,6,-9000,1000
	def move(self,line):
		args = line.split(",")
		if len(args) != 4:
			return False
		
		axis = int(args[1])
		pos = int(args[2])
		vel = int(args[3])
		return MDC.moveto(axis, pos, vel)

	def tx_loop(self):
		while self.alive:
			volts = MDC.get_volts()
			counts = MDC.get_counts()
			self.wfile.write(volts[1] + ',' + ','.join(counts) + '\n')
			time.sleep(0.1)

def write_log():
	f = open("robot.csv", "w")
	f.write("t,c1,p1,d1,c2,p2,d2,c3,p3,d3,c4,p4,d4,c5,p5,d5,c6,p6,d6\n")
	t = 0

	while MDC.axis_count != 6:
		pass

	time.sleep(2)
	print MDC.counts

	while True:
		s = str(t)
		t += 1
		for i in range(1,6+1):
			s += "," + str(MDC.counts[i])
			s += "," + str(MDC.powers[i])
			s += "," + str(MDC.commands[i])
		f.write(s + "\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/ttyACM0", "/dev/ttyACM1", "/dev/ttyACM2"]
	controllers = [MDC.MDC(dev) for dev in devices]

	log_thread = threading.Thread(target = write_log)
	log_thread.daemon = True
	log_thread.start()

	host = 'localhost'
	port = 31415
	server = ServerThread((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.