def MoveLinear(self, x2, y2, z2):
self._logger.info("x2=%f, y2=%f, z2=%f", x2, y2, z2)
mX = Motor(OUTPUT_A)
mY = Motor(OUTPUT_B)
mZ = Motor(OUTPUT_C)
(speedX, speedY) = self.Speed(self.x1, self.y1, x2, y2)
dx = x2 - self.x1
dy = y2 - self.y1
dz = z2 - self.z1
mZ.on_for_degrees(self.speedMax, self.oneMmZ*dz, block=True)
mX.on_for_degrees(speedX, self.oneMmX*dx, block=False)
mY.on_for_degrees(speedY, self.oneMmY*dy, block=False)
mX.wait_until_not_moving()
mY.wait_until_not_moving()
mZ.wait_until_not_moving()
self.x1 = x2
self.y1 = y2
self.z1 = z2
class Hand(object):
def __init__(self, turner, up_motor='B', hand_motor='A'):
self.angle = turner.angle
self.up = Motor(up_motor)
self.hand = Motor(hand_motor)
self.degrees = 21
self.up_bounds = -100
self.pwm_base = 0.1
self.color_org = 0
self.color_tgt = 0
self.up_org = 0
self.up_tgt = 0
def speed(self):
up = self.up.position / self.up_bounds
tgt = self.color_tgt * self.degrees
if up > 0.7:
tgt += 3
pos = self.hand.position / self.hand.count_per_rot * 360
if abs(pos - tgt) < 5:
if abs(pos - tgt) < 1:
return 0.01, tgt
return 0.1, tgt * 1.1
else:
return 5, tgt
def push(self):
vel, pos = self.speed()
self.hand.on_to_position(vel,
pos / 360 * self.hand.count_per_rot,
block=False)
def start(self):
self.hand.position = 0
self.up.position = 0
self.angle.position = 0
self.wait()
def wait(self):
self.up.wait_until_not_moving()
self.angle.wait_until_not_moving()
self.hand.wait_until_not_moving()
def goo(self):
def fun():
while True:
self.push()
sleep(0.05)
self.thread = Thread(target=fun)
self.thread.start()
def stop(self):
self.thread._stop()
def move(self, color, up, angle, lift=False):
self.up_org = self.up_tgt
self.up_tgt = up
self.color_org = self.color_tgt
self.color_tgt = color
self.up.on_to_position(10, up * self.up_bounds, block=False)
self.angle.on_to_position(10,
angle / 360 * self.angle.count_per_rot,
block=False)
self.wait()
