self.moveto(6, cur + 18000, 1000)

- self.moveto(6, cur + 10000, speed)

+ self.moveto(6, cur + 12000, speed)

+ self.moveto(6, cur + 6000, speed)

self.moveto(6, cur, speed)

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

- print [~~int~~(x * 180 / pi) for x in r.fk.theta]

+ print [round(x * 180 / pi, 1) for x in r.fk.theta]

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]