Anonymous avatar Anonymous committed 380da48

Make swing piece-wise, make move_to separate from linear move

Comments (0)

Files changed (1)

 		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 + 15000, 1000)
-		time.sleep(3)
-		self.moveto(6, cur - 15000, speed)
-		time.sleep(10)
+		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 + 10000, 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
 		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[:]
 			continue
 		if line == "hold":
 			r.hold()
-			r.last_xyz = False
-			print "Holding " + str(r.counts) + ": xyz " + str([int(x) for x in r.fk.p])
 			continue
 		if line == "r":
 			print r.counts
 		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)
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.