Source

puma / Shuffleboard.py

Full commit
Trammell Hudson 88ff1bd 












Trammell Hudson 440ed10 
Trammell Hudson 88ff1bd 


Trammell Hudson 96448d6 


tsshuffleboard 946b36e 
tsshuffleboard 7721afc 
tsshuffleboard 946b36e 
tsshuffleboard 7721afc 
Trammell Hudson 8dcbf92 
Trammell Hudson 88ff1bd 

Trammell Hudson 8dcbf92 
Trammell Hudson 88ff1bd 













Trammell Hudson 8dcbf92 
Trammell Hudson 96448d6 
Trammell Hudson 8dcbf92 

tsshuffleboard 946b36e 












Trammell Hudson 440ed10 









Trammell Hudson 8dcbf92 
tsshuffleboard 946b36e 
Trammell Hudson 440ed10 
Trammell Hudson 88ff1bd 













Trammell Hudson 8dcbf92 
Trammell Hudson 88ff1bd 
Trammell Hudson 8dcbf92 
Trammell Hudson 88ff1bd 






Trammell Hudson 8dcbf92 

Trammell Hudson 88ff1bd 





Trammell Hudson 8dcbf92 
Trammell Hudson 88ff1bd 








Trammell Hudson 8dcbf92 
Trammell Hudson 88ff1bd 





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