def pickUpTarget(target):
print "pickUpTarget(target) THIS FUNCTION IS DEPRECATED, remove soon!"
#SET UP
currentPosition = Robot.getPosArgsCopy() #Records position to return to after picking up object
targetFocus = [screenDimensions[0] / 2 - V.pixFromCamToArm, screenDimensions[1] / 2]
focusOnTarget(target, targetFocus = targetFocus)
focusOnTargetManual(target, targetFocus = targetFocus)
Robot.moveTo(height = (V.heightMax + 60) / 4, waitForRobot = True, relative = False)
focusOnTargetManual(target, targetFocus = targetFocus)
#DESCEND ONTO OBJECT
while Robot.getOutput('touch')["touch"] == 1:
if Robot.pos["height"]-V.heightMin > 30:
Robot.moveTo(height = -30)
else:
Robot.moveTo(height = Robot.pos["height"] + V.heightMin)
Robot.setGrabber(1) #Grab
Robot.moveTo(relative = False, **currentPosition) #Return to original position
shapeArray, edgedFrame = objTracker.getShapes(4, returnFrame = True)
movementFrame = objTracker.getMovement(returnFrame = True)[1]
#SET FRAMES FOR THE WINDOWS:
vid.windowFrame["Main"] = objTracker.drawShapes(shapeArray)
vid.windowFrame["Edged"] = edgedFrame
vid.windowFrame["Movement"] = movementFrame
vid.windowFrame["KeyPoints"] = objTracker.drawKeypoints()
#DISPLAY THINGS
vid.display("Main")
vid.display("Edged")
vid.display("Movement")
vid.display("KeyPoints")
ch = cv2.waitKey(10)
keyPressed = chr(ch + (ch == -1) * 256).lower().strip() #Convert ascii to character, and the (ch == -1)*256 is to fix a bug.
if keyPressed == chr(27): exitApp = True #If escape has been pressed
if keyPressed == ord('g'):
print "MAIN: ", Robot.getOutput('all')
if keyPressed == ord(' '): #PAUSE
vid.paused = not vid.paused
#CLOSE EVERYTHING CORRECTLY
robotThread.join(1000) #Ends the robot thread, waits 1 second to do so.
Robot.setGrabber(1)
cv2.destroyWindow('window')
print 'End!'
