Source

puma / Shuffleboard.py

#!/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()
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.