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, sin, cos

r = None

# Coordinate transform the robot Z distance based on the X distance
# This needs to be calibrated if the robot is moved or the tool
# adjusted.
x1 = 450.0
z1 = -15.0
x2 = 170.0
z2 = -37.0

class ShuffleboardSocket(SocketServer.StreamRequestHandler):
	def handle(self):
		print "New client"
		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 map_z(self, x):
		return (z2 - z1) / (x2 - x1) * (x - x1) + z1
		return 0

	def moveto(self,shuffleboard_x,angle):
                # shuffleboard_x = 0 should be at left of shuffleboard table,
                # which is 114 mm robot frame, by measurement
                x = shuffleboard_x + 114
		    
		if x < 170 or x > 450:
		    self.wfile.write("?\r\n")
		    return 0

		if angle < -10 or angle > 10:
		    self.wfile.write("?\r\n")
		    return 0

		# Compute approach vector based on the angle
		# For the swinging tool on the left side of the board,
		# the position is facing (approach) due X,
		# sliding in Z and the normal to Y
		angle = -angle * pi / 180
		a = [ cos(angle), sin(angle), 0]
		s = [ 0,          0,          1]
		n = [-sin(angle), cos(angle), 0]

		# todo: Compute the Z position based on the X distance
		z = self.map_z(x)
		r.move_ik(10000, [x,130,z], a, s, n)

	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)), float(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")
		self.alive = False
		print "Client exited"

	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 = '0.0.0.0'
	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.