Commits

Anonymous committed 946b36e

x and z calibration

Comments (0)

Files changed (2)

 if __name__ == "__main__":
 	import PumaConfig
 	r = Puma(("localhost", 31415), PumaConfig.joints)
-	vel = 20
+	vel = 10
 
 	while r.live:
 		line = sys.stdin.readline()
 # 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 = 400.0
-z1 = -25.0
-x2 = 200.0
-z2 = -35.0
+x1 = 450.0
+z1 = -27.0
+x2 = 170.0
+z2 = -42.0
 
 class ShuffleboardSocket(SocketServer.StreamRequestHandler):
 	def handle(self):
 		return (z2 - z1) / (x2 - x1) * (x - x1) + z1
 		return 0
 
-	def moveto(self,x,angle):
+	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,
 
 		# 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)
+		r.move_ik(10000, [x,130,z], a, s, n)
 
 	def rx_loop(self):
 		while self.alive: