Commits

Trammell Hudson committed 45986e1 Merge

merge with speed limited printout

Comments (0)

Files changed (7)

 		if delta < 0:
 			delta = -delta
 		if ms != 0:
-			moveto(i+1, new_pos, delta * 1000 / ms + 1)
+			moveto(i+1, new_pos, delta * 200 / ms + 1)
 		else:
 			moveto(i+1, new_pos, 0)
 
 	def moveto(self, count, speed=0):
 		if speed is not None and speed != 0:
 			limited = ''
-			if speed > 40000:
-				speed = 40000
+			if speed > 4000:
+				speed = 4000
 				limited = ' !!!'
-			if speed < 10:
-				speed = 10
+			if speed < 1:
+				speed = 1
 				limited = ' !'
 			self.mdc.send("!S " + self.channel + " " + str(int(speed)))
+		if count < -30000:
+			count = -30000
+		elif count > 30000:
+			count = +30000
+
 		print str(self.axis) + ": " + str(int(count)) + " @ " + str(int(speed)) + limited
 		self.mdc.send("!P " + self.channel + " " + str(int(count)))
 		commands[int(self.axis)] = count
 		for channel in ['1','2']:
 			self.send("^CLERD " + channel + " 0")
 			time.sleep(0.1)
-			self.send("^KP " + channel + " 15")
+			self.send("^EPPR " + channel + " 300")
 			time.sleep(0.1)
-			self.send("^KD " + channel + " 5")
+			self.send("^KP " + channel + " 10")
 			time.sleep(0.1)
-			self.send("^KI " + channel + " 1")
+			self.send("^KD " + channel + " 0")
 			time.sleep(0.1)
-			self.send("!s " + channel + " 4000")
+			self.send("^KI " + channel + " 0")
 			time.sleep(0.1)
-			self.send("!ac " + channel + " 60000")
+			self.send("!s " + channel + " 1000")
 			time.sleep(0.1)
-			self.send("!dc " + channel + " 60000")
+			self.send("!ac " + channel + " 15000")
+			time.sleep(0.1)
+			self.send("!dc " + channel + " 15000")
 			time.sleep(0.1)
 
 		# Configure the telemetry stream
 		self.joints = joints
 		self.counts = [0,0,0,0,0]
 		self.last_xyz = False
+		self.volts = 0
 		self.fk = PumaFK(joints)
 		self.ik = PumaIK(False, True, joints)
 		self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
 				break
 			line = line.strip()
 			counts = line.split(",")
-			if len(counts) != 6:
+			if len(counts) != 7:
 				print "Parse error: " + line
 				continue
-			self.counts = [int(x) for x in counts]
+			volts = int(counts[0]) / 10.0
+			self.counts = [int(x) for x in counts[1:]]
 			#print "rx: " + line
 			self.fk.update(self.counts)
 			#print [int(x) for x in r.fk.p]
 
+			if volts < 12.0 and self.volts > 12.0:
+				print "NO VOLTAGE"
+			elif volts > 12.0 and self.volts < 12.0:
+				print "DANGER: Motors are live: " + str(round(volts,2)) + " volts"
+			self.volts = volts
+
 		print "RX THREAD EXITED"
 		self.live = False
 
 		#print cmd
 		self.sock.send(cmd + "\r\n")
 
+	# Move one axis to a set of counts
+	def moveto(self, axis, count, vel):
+		cmd = "move," + str(int(axis)) + "," + str(int(count)) + "," + str(int(vel))
+		self.sock.send(cmd + "\r\n")
+
 	def home(self):
-		self.move_raw(2000, [0,0,0,0,0,0])
+		self.move_raw(10000, [0,0,0,0,0,0])
+	def start(self):
+		self.move_raw(20000, [-417, 10904, -8125, -8109, 295, -6651])
+
+	def hold(self):
+		counts = self.counts[:]
+		for i in range(0,6):
+			self.moveto(i+1, counts[i], 1000)
+		self.last_xyz = False
+		print "Holding " + str(self.counts) + ": xyz " + str([int(x) for x in self.fk.p])
+
+	def swing(self, speed):
+		cur = self.counts[5]
+		self.moveto(6, cur + 6000, 1000)
+		time.sleep(0.5)
+		self.moveto(6, cur + 12000, 1000)
+		time.sleep(0.5)
+		self.moveto(6, cur + 18000, 1000)
+		time.sleep(0.5)
+
+		self.moveto(6, cur + 12000, speed)
+		time.sleep(0.125)
+		self.moveto(6, cur + 6000, speed)
+		time.sleep(0.125)
+		self.moveto(6, cur, speed)
+		time.sleep(0.125)
+		self.moveto(6, cur - 6000, speed)
+		time.sleep(0.125)
+		self.moveto(6, cur - 12000, speed)
+		time.sleep(0.125)
+		self.moveto(6, cur - 18000, speed)
+
+		time.sleep(5)
+		self.moveto(6, cur - 12000, 1000)
+		time.sleep(1)
+		self.moveto(6, cur - 6000, 1000)
+		time.sleep(1)
+		self.moveto(6, cur, 1000)
 
 	# Move immediately to an explicit IK computed position
 	def move_ik(self, ms, xyz, a, s, n):
 
 		print "counts:  ", [int(x) for x in self.counts]
 		print "command: ", [int(x) for x in counts]
-		print "current: ", [int(x* 180/pi) for x in self.fk.theta]
-		print "desired: ", [int(x* 180/pi) for x in self.ik.theta]
+		print "current: ", [round(x* 180/pi, 1) for x in self.fk.theta]
+		print "desired: ", [round(x* 180/pi, 1) for x in self.ik.theta]
 
 	# Move linearly to an IK computed position at mm/s speed
 	# This will hold the current tool configuration
 		else:
 			xyz = self.fk.p[:]
 
+		# 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
+		cur_a = [1,0,0]
+		cur_s = [0,0,1]
+		cur_n = [0,1,0]
+
+		self.move_ik(100000 / vel, dst_xyz, cur_a, cur_s, cur_n)
+		self.last_xyz = dst_xyz[:]
+
+	# Move linearly to an IK computed position at mm/s speed
+	# This will hold the current tool configuration
+	# The current xyz position is the last xyz position sent
+	# or if we have had an intermediate save command, the last
+	# computed FK position.  Otherwise we have a "step" when
+	# the new xyz does not match the current xyz.
+	def move_linear(self, vel, dst_xyz):
+		if self.last_xyz:
+			xyz = self.last_xyz[:]
+		else:
+			xyz = self.fk.p[:]
+
 		cur_a = self.fk.a[:]
 		cur_s = self.fk.s[:]
 		cur_n = self.fk.n[:]
 
-		# Use a tool-down, face forward ASN
-		cur_a = [0,0,-1]
-		cur_s = [0,1,0]
-		cur_n = [-1,0,0]
+		# 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
+		cur_a = [1,0,0]
+		cur_s = [0,0,1]
+		cur_n = [0,1,0]
 
 		dx = dst_xyz[0] - xyz[0]
 		dy = dst_xyz[1] - xyz[1]
 		if line == "home":
 			r.home()
 			continue
+		if line == "start":
+			r.start()
+			continue
+		g = re.match("^swing ([0-9]+)", line)
+		if g is not None:
+			r.swing(int(g.group(1)))
+			continue
 		if line == "hold":
-			r.move_raw(1000, r.counts)
-			r.last_xyz = False
-			print "Holding " + str(r.counts) + ": xyz " + str([int(x) for x in r.fk.p])
+			r.hold()
 			continue
 		if line == "r":
 			print r.counts
 			continue
 		if line == "a":
-			print [int(x * 180 / pi) for x in r.fk.theta]
+			print [round(x * 180 / pi, 1) for x in r.fk.theta]
 			continue
 		if line == "p":
 			print [int(x) for x in r.fk.p], [round(x,2) for x in r.fk.a], [round(x,2) for x in r.fk.s], [round(x,2) for x in r.fk.n]
 		g = re.match("^v ([0-9]+)", line)
 		if g is not None:
 			vel = int(g.group(1))
+			if (vel == 0):
+				vel = 1
 			continue
 
 		g = re.match("^(-?[0-9]+ -?[0-9]+ -?[0-9]+)$", line)
 		if g is not None:
 			xyz = [int(x) for x in g.group(1).split(" ")]
 			r.move_to(vel, xyz)
+			continue;
+
+		print "?"
 from Joint import Joint
 
 joints = [
-	Joint(0, 0, 0, -11200 - +12000, 0),
-	Joint(130, 205, -90, +18000 - -18000, 0),
-	Joint(0, 0, +90, -10500 - +11200, 0),
+	Joint(0, 0, 0, -11500 - +12000, 0),
+#	Joint(130, 205, -90, +18000 - -18000, 0),
+	Joint(130, 205, -90, +17800 - -17000, 0),
+#	Joint(0, 0, +90, -10500 - +11200, 0),
+	Joint(0, 0, +90, -11200 - +10500, 0),
 	Joint(225, 0, -90, +17400, 0),
-	Joint(0, 0, 0, -8000 - +8000, -6917/30674.0),
-	Joint(50, 0, 0, +6500 - -6500, 1200/-10000.0),
+	Joint(0, 0, 0, -7800 - +7800, -6917/30674.0),
+#	Joint(50, 0, 0, +6250 - -6250, 1425/-10000.0),
+	Joint(50, 0, 0, +6250 - -6250, 1425/-10000.0),
 ]
 			j = self.joints[i]
 			a = self.theta[i]
 
+			# Elbow joint is off by 90 deg
+			if i == 2 and a < 0:
+				a += 2 * pi
+
 			# try adjusting to avoid a wrap around
 			# on the wrist joints that can do more than
 			# 360 degrees.
-			if True or i == 3 or i == 5:
+			if False and (i == 3 or i == 5):
 				if a - current_theta[i] > 1.5*pi:
 					a -= 2 * pi
 				elif a - current_theta[i] < -1.5*pi:
+#!/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
+
+# Map x distance to height
+# 250 -45
+# 300 -40
+# 350 -35
+# 400 -30
+# 450 -25
+
+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):
+		if (x >= 400 ): return -25
+		if (x >= 350 ): return -30
+		if (x >= 300 ): return -35
+		if (x >= 250 ): return -40
+		if (x >= 200 ): return -40
+		return 0
+
+	def moveto(self,x,angle):
+		# 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(5000, [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()
 					self.wfile.write("! go error\n")
 				continue
 
+			if line.startswith("move,"):
+				if not self.move(line):
+					self.wfile.write("! move error\n")
+				continue
+
 			m = re.match('^([0-9]),(-?[0-9]+)(?:,([0-9]+))?$', line)
 			if m is None:
 				self.wfile.write("! parse error\n")
 		MDC.move_all(ms, dest)
 		return True
 
+	# Parse a "move" command with the one joint number,
+	# one position and one speed:
+	# move,6,-9000,1000
+	def move(self,line):
+		args = line.split(",")
+		if len(args) != 4:
+			return False
+		
+		axis = int(args[1])
+		pos = int(args[2])
+		vel = int(args[3])
+		return MDC.moveto(axis, pos, vel)
+
 	def tx_loop(self):
 		while self.alive:
-			#counts = MDC.get_volts()
+			volts = MDC.get_volts()
 			counts = MDC.get_counts()
-			self.wfile.write(','.join(counts) + '\n')
+			self.wfile.write(volts[1] + ',' + ','.join(counts) + '\n')
 			time.sleep(0.1)
 
 def write_log():
 	daemon_threads = True
 	allow_reuse_address = True
 
+
 if __name__ == "__main__":
-	devices = ["/dev/tty.usbmodemfd1231", "/dev/tty.usbmodemfd1241", "/dev/tty.usbmodemfd1271"]
+	devices = ["/dev/tty.usbmodem1d1131", "/dev/tty.usbmodem1d1141", "/dev/tty.usbmodem1d1171"]
 	controllers = [MDC.MDC(dev) for dev in devices]
 
 	log_thread = threading.Thread(target = write_log)
+#!/usr/bin/python
+# Play a quick round of shuffleboard to show how it is done
+import PumaConfig
+import Puma
+import sys
+import time
+
+r = Puma.Puma(("localhost", 31415), PumaConfig.joints)
+vel = 10
+power0 = 1550
+power1 = 1600
+power2 = 1560
+power3 = 1560
+time.sleep(2)
+
+r.hold()
+
+raw_input("Station 1")
+r.move_to(vel, [400, 130, -85])
+
+raw_input("Swing 1!")
+r.swing(power0)
+
+raw_input("Swing 2!")
+r.swing(power1)
+
+raw_input("Station 2")
+r.move_to(vel, [300, 130, -85])
+
+raw_input("Swing 1!")
+r.swing(power2)
+
+raw_input("Station 3")
+r.move_to(vel, [430, 130, -80])
+
+raw_input("Swing 1!")
+r.swing(power3)
+
+raw_input("Go home")
+r.home()
+
+
+