Source

puma / mdc-telnet.py

Full commit
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 edeff5a 
Trammell Hudson e4c0d32 










Trammell Hudson b3bce6e 




Trammell Hudson f7e2257 




Trammell Hudson b3bce6e 
Trammell Hudson a6c5766 







Trammell Hudson 08ec878 
Trammell Hudson a6c5766 




Trammell Hudson b3bce6e 







Trammell Hudson aef2c9d 

Trammell Hudson b3bce6e 

Trammell Hudson f7e2257 












Trammell Hudson 5ffbf16 
Trammell Hudson a6c5766 
Trammell Hudson 4b982b4 
Trammell Hudson 6a3ec93 
Trammell Hudson 4b982b4 
Trammell Hudson e4c0d32 
Trammell Hudson 08ec878 
Trammell Hudson edeff5a 





















Trammell Hudson 0e58d93 




Trammell Hudson 88ff1bd 
Trammell Hudson 08ec878 
Trammell Hudson 3f53ea9 

Trammell Hudson 874e48b 

Trammell Hudson edeff5a 



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

			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"]
	devices = ["/dev/tty.usbmodem1d1131", "/dev/tty.usbmodem1d1141", "/dev/tty.usbmodem1d1171"];
	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()