'''
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)
