''' Note that the coordinate frame turns as the robot turns. Forward and backback should really be defined in terms of the robots current position and not world x,y and z. But hey, this works as a demo. ''' from arm import Arm from control import Control def move(arm, x, y, z): arm.step(x=x, y=y, z=z, wait=True) def pump(arm, state): arm.grab(state) my_arm = Arm.connect() move(arm, 189, -85, 70) pump(arm, True) while True: sleep(1)
import numpy from arm import Arm from board import Board a = Arm.connect() b = Board.connect(verbose=True) while 1: pot = b.get_pot() pho = b.get_photo() fx = round(numpy.interp(pot, [0, 1], [190, 210])) fy = round(numpy.interp(pot, [0, 1], [-200, 200])) fz = round(numpy.interp(pot, [0, 0.5, 1], [50, 300, 50])) print(pot, fx, fy, fz) a.goto(fx, fy, fz, wait=True) if pho > 0.8: a.grab(False) if pho < 0.8: a.grab(True)