Commits

Trammell Hudson committed a6c5766

parse commands from telnet port

Comments (0)

Files changed (2)

 			s.append(str(counts[axis]))
 	return s
 
+def moveto(axis,pos,vel):
+	if axis not in axises:
+		return False
+	chan = axises[axis]
+	chan.moveto(pos, vel)
+	return True
+
 
 class MDC_channel:
 	def __init__(self, mdc, channel, axis):
 		self.axis = axis
 
 	def moveto(self, count, speed=0):
-		if speed != 0:
+		if speed is not None and speed != 0:
 			self.mdc.send("!S " + self.channel + " " + str(int(speed)))
-		self.mdc.send("!P " + self.channel + str(int(count)))
+		self.mdc.send("!P " + self.channel + " " + str(int(count)))
 
 class MDC:
 	def __init__(self, tty):
 #!/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.re = re.compile('^([0-9]),(-?[0-9]+)(?:,([0-9]+))?$')
+		self.alive = True
 		self.timeout = 1
 		self.thread = threading.Thread(target = self.output_periodic)
 		self.thread.daemon = True
 		self.thread.start()
 
 		while True:
-			line = self.rfile.readline()
+			line = self.rfile.readline().strip()
 			if not line:
 				break
-			print "Read line" + line
+			print "Read line '" + line + "'"
+			m = self.re.match(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)
 
-		self.thread.stop()
+			if not MDC.moveto(axis, pos, vel):
+				self.wfile.write("! bad axis " + str(axis) + "\n")
+
+		self.alive = False
+		print "Client exited"
 	
 	def output_periodic(self):
-		while True:
+		while self.alive:
 			self.wfile.write(','.join(MDC.get_counts()) + '\n')
 			time.sleep(1)