Commits

Anonymous committed 440ed10

Angle computation works

Comments (0)

Files changed (1)

 import sys
 import SocketServer
 from Puma import Puma
-from math import sqrt, pi
+from math import sqrt, pi, sin, cos
 
 r = None
 
 		# why doesn't this work as a thread?
 		self.rx_loop()
 
+	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
+		r.move_ik(5000, [x,130,0], a, s, n)
+
 	def rx_loop(self):
 		while self.alive:
 			line = self.rfile.readline().strip()
 				r.hold()
 				continue
 
-			g = re.match("^move ([0-9]+) ([0-9]+)", line)
+			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