# HG changeset patch # User Trammell Hudson # Date 1376408975 14400 # Node ID 380da483407f0a2d721ac5bf6c03c1363df7dc86 # Parent 0363214c1ea1dd471e4a589df526f63e4e3d7567 Make swing piece-wise, make move_to separate from linear move diff --git a/Puma.py b/Puma.py --- a/Puma.py +++ b/Puma.py @@ -72,13 +72,33 @@ 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 @@ -105,6 +125,28 @@ 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[:] @@ -173,8 +215,6 @@ 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 @@ -188,6 +228,8 @@ 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)