def placeBlock(position, height, wrist):
currentPosition = Robot.getPosArgsCopy()
Robot.moveTo(rotation = position["rotation"], stretch = position["stretch"], relative = False) #GET IN POSITION OVER DROP ZONE
Robot.moveTo(height = height, wrist = wrist, waitForRobot = True, relative = False)
Robot.setGrabber(0)
Robot.moveTo(height = currentPosition["height"], relative = False)
Robot.moveTo(relative = False, **currentPosition)
def runRobot():
sleep(3)
#SET UP VARIABLES AND RESET ROBOT POSITION
print "runRobot(", locals().get("args"), "): Setting up Robot Thread...."
global exitApp
global keyPressed #Is set in the main function, because CV2 can only read keypresses in the main function. Is a character.
Robot.moveTo(relative = False, waitForRobot = True, **Robot.home)
Robot.setGrabber(0)
#MAIN ROBOT FUNCTION
while not exitApp:
if keyPressed == 'c': #CALIBRATION COMMAND
r, s = calibrateRobotCamera(0)
print 'Distance r: %s s: %s' % (r, s)
if keyPressed == 'h': #HELP COMMAND
print "Help: \n c: CALLIBRATION \n j: STACKJENGA \n m: DETECT MOVEMENT",
print "\n p: PICKUPTARGET \n r: RESETPOS,"
print "\n t: TARGET \n x: MOVETOXY"
if keyPressed == 'j': #CALIBRATION COMMAND
stackJenga()
if keyPressed == 'm': #DETECT MOVEMENT
print "Movement: ", objTracker.getMovement()[0]
print "Is object grabbed: ", isObjectGrabbed()
if keyPressed == 'p': #PICK UP TARGET
pickUpBlock(isObjectGrabbed(), Robot.getPosArgsCopy())
if keyPressed == 'r': #RESET
Robot.moveTo(relative = False, waitForRobot = True, **Robot.home)
Robot.setGrabber(0)
if keyPressed == 's': #GET ANGLE OF OBJECT ON SCREEN CLOSEST TO CENTER
print getAngle(objTracker.bruteGetFrame(lambda: objTracker.getNearestShape(4).vertices))
if keyPressed == 't': #TARGET
try:
targetFocus = [screenDimensions[0] * .75 + int(raw_input("Shift x by?:")), screenDimensions[1] / 2 + int(raw_input("Shift y by?:"))]
focusOnTarget(lambda: objTracker.bruteGetFrame(lambda: objTracker.getNearestShape(4, nearestTo = targetFocus).center), targetFocus = targetFocus, tolerance = 3, **{"ppX": 3.7, "ppY": 15})
except Exception as e:
print e
#targetFocus = [screenDimensions[0] / 2, screenDimensions[1] / 2]
#focusOnTarget(lambda: objTracker.bruteGetFrame(lambda: objTracker.getNearestShape(4, nearestTo = targetFocus).center), targetFocus = targetFocus, ppX = 5.3, ppY = 38)
if keyPressed == 'x': #MOVE TO XY
#Robot.moveTo(x = float(raw_input('x = ')), y = float(raw_input('y = ')), height = 10, relative = False) #POLAR COORDINATES
#Robot.moveTo(height = float(raw_input("height?:")), rotation = float(raw_input('Rotation?: ')), stretch = float(raw_input('Stretch?: ')), relative = False) #ROTATION / STRETCH
Robot.moveTo(height = float(raw_input("height?:")), rotation = 33, stretch = 100, relative = False)
def placeBlockAtPosition(layer, position):
"""
FOR THE TOWER BUILDING. Aligns the brick to either a horizontal or vertical position, determined mathematically by the "layer" variable.
The "layer" variable determines the height.
The "position" variable is a string- it is either 0, 1, 2 determining where on the current layer of the tower (1 = left, 2 = middle, or something akin).
that the brick should go on.
:param layer: Layer on the building. Starts at 1. 1st layer is horizontal.
:param position: "right", "left
:return: A brick placed
"""
#SET UP VARIABLES
currentPosition = Robot.getPosArgsCopy() #Back up the position in case of error
heightForLayer = {"1": -32, "2": -15, "3": 3, "4": 24, "5": 40.5}
print "placeBlockAtPosition(", locals().get("args"), "): Current pos: ", currentPosition
#PICK THE RELEVANT MOVEMENTS FOR EACH LAYER/POSITION, AND PERFORM IT
if not layer % 2: #IF THIS IS THE VERTICAL LAYER
print "PLACING DOWN VERTICAL BLOCK ON POSITION ", position, "ON LAYER ", layer
#HOW MUCH STRETCH / ROTATION IS NECESSARY FOR EACH POSITION. [POSITION]
rotToMoveSide = [19, 0, -19]
rotToMoveSlip = [-12, 0, 10]
stretchToMove = [-2, 0, -2] #Adjust the angle so it is truly vertical.
wristToMove = [-10, 0, 10]
Robot.moveTo(height = heightForLayer[str(layer)] + 34, wrist = wristToMove[position], relative = False) #Move Wrist BEFORE going all the way down, or else you might knock some blocks
Robot.moveTo(rotation = rotToMoveSide[position], stretch = stretchToMove[position]) #Go besides (but offset) to the end position of the block
sleep(.1)
Robot.moveTo(height = heightForLayer[str(layer)], relative = False) #Finish going down
waitTillStill()
Robot.moveTo(rotation = rotToMoveSlip[position] * .75) #Slip block in to the correct position sideways (Hopefully pushing other blocks into a better position
sleep(.1)
Robot.moveTo(rotation = rotToMoveSlip[position] * .25) #Slip block in to the correct position sideways (Hopefully pushing other blocks into a better position
else: #IF THIS IS THE HORIZONTAL LAYER
stretchToMoveSide = [-45, 0, 49] #To get parallel to the part
stretchToMoveSlip = [25, 0, -25] #To slip it in all the way
wristToMove = 73.5 #Equivalent to 90 degrees, for some reason
Robot.moveTo(height = heightForLayer[str(layer)] + 34, wrist = wristToMove, relative = False) #Move Wrist BEFORE going all the way down, or else you might knock some blocks
Robot.moveTo(stretch = stretchToMoveSide[position])
sleep(.1)
Robot.moveTo(height = heightForLayer[str(layer)], relative = False) #Finish going down
waitTillStill()
Robot.moveTo(stretch = stretchToMoveSlip[position] * .75) #Ease into the correct position by splitting it into two moves
sleep(.1)
Robot.moveTo(stretch = stretchToMoveSlip[position] * .25)
#DROP BRICK AND GO BACK TO ORIGINAL POSITION
sleep(.1)
print Robot.pos
Robot.setGrabber(0)
Robot.moveTo(height = currentPosition["height"], relative = False)
sleep(.2)
Robot.moveTo(relative = False, **currentPosition)
waitTillStill()
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
def pickUpBlock(movConstant, objectLastSeen, **kwargs):
"""
Robot will attempt to pick up block. If it fails, it will RECURSIVELY attempt to pick it up again from the last known position. kwarg "attempts" is used to cut off the robot after
the 3rd (configurable) attempt.
:param movementConstant: this is the average pixel change per frame when the robot is stationary and there is no jenga block in it's grabber when the grabber moves.
It is used with isObjectGrabbed to detect if the robot successfuly grabbed a jenga block
:param startingPos: Self explanatory. String of robot position, from where to start
:return:
kwargs:
attempts: Counts how many times before this program has run itself recursively, in order to determine if it should stop attempting and return an error.
attemptRefocus: (Defaults to true). If this is true, the robot will attempt to refocus on the block using a different manner if it is >50 or <-50 degrees position.
"""
attempts = kwargs.get("attempts", 0) #Total times this func has been run recursively (at 3, it raises an error)
attemptRefocus = kwargs.get("attemptRefocus", True) #Should robot attempt to do fancy refocusing on certain blocks
targetCenter = [screenDimensions[0] / 2, screenDimensions[1] / 2] #Target center of screen
heightSettings = {"150": {"ppX": 3, "ppY": 12}, "70": {"ppX": 3.7, "ppY": 15}, "0": {"ppX": 5.3, "ppY": -1}} #Holds the best ppX ppY settings for different heights.
getShapeCenter = lambda: objTracker.bruteGetFrame(lambda: objTracker.getNearestShape(4).center) #This function is used a lot, so it was useful to save as a variable.
#CHECK IF THIS FUNCTION HAS BEEN RUN RECURSIVELY MORE THAN THE SET LIMIT, AND QUIT IF IT HAS
print "pickUpBlock(", locals().get("args"), "): Attempt number: ", attempts
if attempts == 3: #If max attempts have been hit
print "pickUpBlock(", locals().get("args"), "): Too many recursive attempts. Raising error"
raise Exception("BlockNotGrabbed")
#GET ROBOT INTO POSITION AND CHECK IF THE OBJECT IS STILL THERE. QUIT IF IT IS NOT.
waitTillStill()
Robot.setGrabber(0)
Robot.moveTo(relative = False, **objectLastSeen)
waitTillStill()
sleep(.1)
if len(objTracker.getShapes(4)) == 0: #If no objects in view
print "pickUpBlock(", locals().get("args"), "): No objects seen at start. Raising error..."
raise NameError("ObjNotFound")
#BEGIN FOCUSING PROCESS UNTIL ROBOT IS AT 0 HEIGHT AND COMPLETELY FOCUSED ON OBJECT
try:
if Robot.pos["height"] == 150:
print "pickUpBlock(", locals().get("args"), "): Focus at height 150"
objectLastSeen = Robot.getPosArgsCopy(dontRecord = ["wrist, grabber, height"])
focusOnTarget(getShapeCenter, **heightSettings["150"]) #Tries many times (brute) to get the nearest shapes .center coords & focus
objectLastSeen = Robot.getPosArgsCopy(dontRecord = ["wrist, grabber, height"])
Robot.moveTo(height = 70, relative = False)
waitTillStill()
if Robot.pos["height"] == 70:
print "pickUpBlock(", locals().get("args"), "): Focus at height 70"
focusOnTarget(getShapeCenter, **heightSettings["70"])
objectLastSeen = Robot.getPosArgsCopy(dontRecord = ["wrist, grabber, height"])
Robot.moveTo(height = 0, relative = False)
waitTillStill()
if Robot.pos["height"] == 0:
print "pickUpBlock(", locals().get("args"), "): Focus at height 0"
focusOnTarget(getShapeCenter, tolerance = 7, **heightSettings["0"])
objectLastSeen = Robot.getPosArgsCopy(dontRecord = ["wrist, grabber, height"])
shape = objTracker.bruteGetFrame(lambda: objTracker.getNearestShape(4)).vertices
except NameError as e:
print "ERROR: pickUpBlock(", locals().get("args"), "): ", e
pickUpBlock(movConstant, objectLastSeen, attempts = attempts + 1)
return False
#raise Exception("PickupFailed")
#IF THE OBJECT IS > 50 OR < -50 DEGREES, DO ANOTHER ROUND OF FOCUSING ON IT, AND PERFORM A DIFFERENT "DROP DOWN" MANUEVER
angle = getAngle(shape) #Get angle of object in camera
if (angle > 50 or angle < -50) and attemptRefocus:
try:
print "pickUpBlock(", locals().get("args"), "): Performing re-focusing manuever on block of angle: ", angle
targetFocus = [screenDimensions[0] / 3.5, screenDimensions[1] / 2]
focusOnTarget(lambda: objTracker.bruteGetFrame(lambda: objTracker.getNearestShape(4, nearestTo = targetFocus).center), tolerance = 8, targetFocus = targetFocus, **heightSettings["0"])
objectLastSeen = Robot.getPosArgsCopy(dontRecord = ["wrist, grabber, height"])
except NameError as e: #Since it failed, try again but this time send the function a "normalPickup = True", so it won't attempt to do it again
print "ERROR: pickUpBlock(", locals().get("args"), "): ", e, " when trying to RE-FOCUS on a >50 <-50 block"
pickUpBlock(movConstant, objectLastSeen, attempts = attempts + 1, attemptRefocus = False)
return False
#MOVE SO OBJECT IS UNDER GRABBER
Robot.moveTo(height = -05, relative = False) #Ease into it
waitTillStill()
Robot.moveTo(stretch = 26)
waitTillStill()
else:
#MOVE SO OBJECT IS UNDER GRABBER
Robot.moveTo(stretch = 52, rotation = 2.25) #Added a rotation to fix weird issues that had been occurring...
waitTillStill()
#MOVE WRIST, AND DESCEND ONTO OBJECT AND PICK IT UP
Robot.moveTo(wrist = angle, height = -20, relative = False)
sleep(.075)
Robot.moveTo(height = -38, relative = False)
sleep(.05)
Robot.setGrabber(1) #Pick up object
sleep(.125)
Robot.moveTo(height = -5, relative = False)
sleep(.2)
#MEASURE THE MOVEMENT OF THE OBJECT AS THE WRIST MOVES
Robot.moveTo(wrist = -40, relative = False) #Start rotating the wrist
Robot.moveTo(wrist = 20, relative = False)
timer = Common.Timer(.4)
highestMovement = 0
while not timer.timeIsUp(): #Gets the highest measured movement in .3 seconds, to DEFINITELY catch the wrist moving. Eliminates problems.
newMovement = objTracker.getMovement()
if newMovement > highestMovement: highestMovement = newMovement
if highestMovement > movConstant + 1.5: break
print "pickUpBlock(", locals().get("args"), "): highestMovement: ", highestMovement
#IF MOVEMENT IS < MOVCONSTANT, THEN NO OBJECT WAS PICKED UP. RE-RUN FUNCTION.
if highestMovement < movConstant + 3:
print "pickUpBlock(", locals().get("args"), "): Failed to suck in object. Retrying..."
pickUpBlock(movConstant, objectLastSeen, attempts = attempts + 1, attemptRefocus = not attemptRefocus)
return False
Robot.moveTo(wrist = 0, relative = False) #Return wrist
return True
def stackJenga(): #Main Function
print "Playing Jenga!"
#SET UP VARIABLES AND RESET ROBOT POSITION
blocks = (1, 0, 2) #The order to place the blocks
markerLKP = {"rotation": 30, "stretch": 100} #Keep track of the "marker block"'s "Last Known Position" (LKP)
searchPos = {'rotation': -15, 'stretch': V.stretchMax / 2.5, 'height': V.heightMax, 'wrist': 0}
originalMarker = markerLKP.copy()
originalSearchPos = searchPos.copy()
Robot.moveTo(relative = False, waitForRobot = True, **searchPos) #Get robot in position before testing movementConstant
movementConstant = (isObjectGrabbed() + isObjectGrabbed() + isObjectGrabbed()) / 3 #Get an average of how much movement there is when there is no block
for l in (1, 2, 3, 4, 5): #For each layer
#FIND A BLOCK, PICK UP THE BLOCK, AND PLACE IT IN THE CORRECT SPOT
b = 0 #Current block you are putting. THE ORDER IS IN THE "blocks" ARRAY, HOWEVER!!!
while b in blocks: #Blocks to place, in order
print "stackJenga(XXX): Currently on layer ", l, " and on block ", b
#GO TO POSITION FOR SEARCHING FOR BLOCKS, BUT DO IT SLOWLY AS TO NOT MOVE ROBOT'S BASE
sleep(.2)
Robot.moveTo(relative = False, waitForRobot = True, **searchPos)
#START SEARCHING FOR BLOCKS
while len(objTracker.getShapes(4)) == 0: #Search for a view that has a block in it
if Robot.pos["rotation"] > -69:
Robot.moveTo(rotation = -10)
waitTillStill()
else:
print "stackJenga(", locals().get("args"), "): No shapes found after one sweep."
searchPos = originalSearchPos.copy()
Robot.moveTo(rotation = -15, stretch = V.stretchMax / 2.5, height = V.heightMax, relative = False) #Go back and sweep again
searchPos = {"rotation": Robot.pos["rotation"], "stretch": V.stretchMax / 2.5, "height": V.heightMax}
#PICK UP A BLOCK
try:
pickUpBlock(movementConstant, Robot.getPosArgsCopy()) #IF pickUpBlock() FAILS 3 ATTEMPTS, RESTART THIS FOR LOOP
except Exception as e:
Robot.setGrabber(0)
print "ERROR: stackJenga(XXX): ", e, " while picking up block. Restarting for loop and attempting again.."
continue #Since b never got upped one, then there is no harm in using continue
#MOVE TO, FOCUS ON, AND RECORD NEW POSITION, OF THE MARKER BLOCK
Robot.moveTo(rotation = markerLKP["rotation"] / 2, height = 70, relative = False)
sleep(.2)
Robot.moveTo(rotation = markerLKP["rotation"], stretch = markerLKP["stretch"], height = 70, relative = False, waitForRobot = True)
waitTillStill()
error = False
if len(objTracker.getShapes(4)) == 0: #If marker not seen in the middle, go to the right and check for it
Robot.moveTo(rotation = markerLKP["rotation"] + 10, stretch = 100, relative = False, waitForRobot = True)
if len(objTracker.getShapes(4)) == 0: #If marker still not to the right, move to the left and check for it
Robot.moveTo(rotation = markerLKP["rotation"] - 10, stretch = 100, relative = False, waitForRobot = True)
if len(objTracker.getShapes(4)) > 0: #If the marker HAS been seen, then focus on it.
try:
targetFocus = [screenDimensions[0] * .75, screenDimensions[1] / 2] #The right edge of the screen
focusOnTarget(lambda: objTracker.bruteGetFrame(lambda: objTracker.getNearestShape(4, nearestTo = targetFocus ).center), targetFocus = targetFocus, tolerance = 9, **{"ppX": 3.7, "ppY": 15})
print "stackJenga(XXX): Successfully focused on marker block"
except Exception as e:
print "ERROR: stackJenga(XXX): ", e, " while searching for marker."
error = True
else:
error = True #If the marker has NOT been seen, catch it right below
if error: #If the robot messed up either focusing on the object or having never seen it, then...
print "stackJenga(XXX): Failed too many times searching for marker. Placing current off to the side, RESETTING markerLKP"
placeBlock(searchPos, 0, 0)
markerLKP = originalMarker.copy()
continue
waitTillStill()
#RECORD NEW ADJUSTED "LAST KNOWN POSITION" OF markerLKP, THROUGH AN AVG OF LAST AND CURRENT.
if abs(Robot.pos["rotation"] - markerLKP["rotation"]) < 20 and abs(Robot.pos["stretch"] - markerLKP["stretch"]) < 30 and not (l == 1 and b == 0): #If the marker is in a semi-valid position, then RECORD that position. (used to be a big issue)
markerLKP = {"rotation": (markerLKP["rotation"] + Robot.pos["rotation"]) / 2, "stretch": (markerLKP["stretch"] + Robot.pos["stretch"]) / 2} #Get avg of last known position and new known position (messes things up less)
print "stackJenga(XXX): New location of markerLKP: ", markerLKP #Print new location of the marker
else:
print "stackJenga(XXX): SOMETHINGS GONE WRONG with markerLKP! markerLKP: ", markerLKP
#PLACE BLOCK ONTO THE CORRECT POSITION ON THE STACK
placeBlockAtPosition(l, blocks[b])
b += 1 #Mark block on layer as completed, so for loop will move on to next block.
#MISSION COMPLETE! INITIATE DANCE SEQUENCE!
Robot.moveTo(relative = False, **Robot.home)
sleep(.6)
Robot.moveTo(rotation = -30)
sleep(.3)
Robot.moveTo(rotation = 30)
sleep(.3)
Robot.moveTo(height = 0)
sleep(.3)
Robot.moveTo(height = V.heightMax)
sleep(.3)
