Anonymous avatar Anonymous committed 88ff1bd

Shuffleboard server, almost works

Comments (0)

Files changed (2)

+#!/usr/bin/python
+#
+# Shuffleboard Puma robot server.
+# Depends on mdc-telnet.py being running
+#
+import socket
+import threading
+import time
+import os
+import re
+import sys
+import SocketServer
+from Puma import Puma
+from math import sqrt, pi
+
+r = None
+
+class ShuffleboardSocket(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 == "home":
+				r.home()
+				continue
+			if line == "hold":
+				r.hold()
+				continue
+
+			g = re.match("^move ([0-9]+) ([0-9]+)", line)
+			if g is not None:
+				self.moveto(int(g.group(1)), int(g.group(2)))
+				continue
+			g = re.match("^swing ([0-9]+)", line)
+			if g is not None:
+				r.swing(int(g.group(1)))
+				continue
+
+			self.wfile.write("?\r\n")
+
+	def tx_loop(self):
+		while self.alive:
+			volts = r.volts
+			pos = [str(round(x,2)) for x in r.fk.p]
+			speed = str(r.counts[5])
+			self.wfile.write(str(round(volts,1)) + ',' + ','.join(pos) + ',' + speed + '\n')
+			time.sleep(0.5)
+
+
+class ServerThread(SocketServer.ThreadingMixIn, SocketServer.TCPServer):
+	# Force SOREUSEADDR allways
+	daemon_threads = True
+	allow_reuse_address = True
+
+if __name__ == "__main__":
+	host = 'localhost'
+	port = 27189
+	server = ServerThread((host,port), ShuffleboardSocket)
+
+	import PumaConfig
+	r = Puma(("localhost", 31415), PumaConfig.joints)
+
+	server.serve_forever()
 	daemon_threads = True
 	allow_reuse_address = True
 
+
 if __name__ == "__main__":
 	devices = ["/dev/tty.usbmodem1d1131", "/dev/tty.usbmodem1d1141", "/dev/tty.usbmodem1d1171"]
 	controllers = [MDC.MDC(dev) for dev in devices]
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.