def driveToColor(color): print("Running drive to color function") camera_update() msleep(2000) while get_object_count(color) == 0: camera_update() msleep(100) d.rotate(1) print("saw object") msleep(500) # Note that the "center" of your x/y camera view is actually around 80. # The furthest-left value is 0, the furthest right value is around 160 # As it is written, I would expect your code to attempt to keep the can in the # "corner" of your robot's vision, as it moves towards it. 5 and 15 are not # good values for "centering" the can -LMB while (get_object_center_x(color, 0) > 15) or (get_object_center_x( color, 0) < 5): print(get_object_center_x(color, 0)) camera_update() msleep(100) if get_object_center_x(color, 0) < 5: d.rotate(-1) elif get_object_center_x(color, 0) > 15: d.rotate(1) print("Centered can") print(get_object_center_x(color, 0)) msleep(500) while get_object_center_y(color, 0) < 60: print(get_object_center_y(color, 0)) d.driveTimed(10, 10, 20) print("Got close to can")
def disposeOfDirt(): driveTimed(95, 100, 500) moveClaw(c.clawClose, 20) moveArm(c.armBack, 25) moveClaw(c.clawOpen, 10) msleep(100) moveClaw(c.clawMid, 15)
def deliverBin(): print("deliverBin") drive(-100, 0) while not onBlack(c.STARBOARD_TOPHAT): pass stop() driveTimed(100, 100, 500)
def smoothLineFollowLeft(time, speed): #Max speed is 80 #Proportional adjustment LF sec = seconds() + time while seconds() < sec: num = ((w.analog(4) - 1500) / 75) d.driveTimed(speed - num, speed + num, 10)
def timedLineFollowLeftSmooth(time): sec = seconds() + time while seconds() < sec: if onBlackFront(): d.driveTimed(20, 40, 20) else: d.driveTimed(40, 20, 20) msleep(10)
def timedLineFollowRight(time): sec = seconds() + time while seconds() < sec: if not onBlackFront(): d.driveTimed(20, 90, 20) else: d.driveTimed(90, 20, 20) msleep(10)
def timedLineFollowLeftBack(time): # follows on starboard side sec = seconds() + time while seconds() < sec: if onBlackBack(): d.driveTimed(-90, -20, 20) else: d.driveTimed(-20, -90, 20) msleep(10)
def releaseBin(): print ("releaseBin") moveArm(c.armUp, 15) binGrabDown() print "Dropped bin at:" currentTime() driveTimed(100, 80, 1000) moveOutrigger(c.outriggerIn, 100)
def lineFollowRightSmoothCount(amount): count = 0 while count < amount: if not onBlackFront(): d.driveTimed(10, 30, 10) count = count + 1 else: d.driveTimed(30, 10, 10) count = 0
def goToHome (): print("goToHome") turnUntilBlack(c.STARBOARD_TOPHAT, 100, 0) lineFollowUntilEndLeft2(c.STARBOARD_TOPHAT) driveTimed(1000, 100, 250) moveOutrigger(c.outriggerSpin, 100) if c.isPrime: timedLineFollowRight(c.LINE_FOLLOWER, 2.5) else: timedLineFollowRight(c.LINE_FOLLOWER, 3)
def grabBin(): print("grabBin") moveOutrigger(c.outriggerBin, 20) driveTimed(0, -100, 500) drive(-95, -100) while not onBlack(c.STARBOARD_TOPHAT): pass stop() driveTimed(-50, -50, 150) binGrabUp()
def turnToSouth(): print("turnToSouth") driveTimed(90, 100, 1000) deliverPoms() drive(100, 0) while not onBlack(c.STARBOARD_TOPHAT): pass stop() driveTimed(100, 0, 500) moveOutrigger(c.outriggerOut, 100)
def grabCan(): u.moveServo(c.servoClaw, c.clawOpen, 20) msleep(200) u.moveServo(c.servoArm, c.armLevel, 20) msleep(500) d.driveTimed(80, 80, 1000) u.moveServo(c.servoClaw, c.clawOpen90, 10) u.moveServo(c.servoClaw, c.grabCan, 5) msleep(200) u.moveServo(c.servoArm, 1200, 10)
def scoreCube(): print "scoreCube" drive(50, 50) while not onBlack(c.STARBOARD_TOPHAT): pass stop() msleep(500) driveTimed(45, 50, 700) moveClaw(c.clawOpen, 25) msleep(300) moveArm(c.armUp, 10) driveTimed(100, 100, 1100)
def grabCube(): print("grabCube") moveArm(c.armUp, 10) drive(0, -100) crossBlack(c.LINE_FOLLOWER) moveClaw(c.clawOpen, 15) msleep(500) stop() moveArm(c.armFront, 15) driveTimed(100, 100, 1100) binGrabUp() moveClaw(c.clawClose, 10)
def driveSquare(size): '''x = 4 while x > 0: driveTimed(100, 100, size) rotate(90) msleep(500) x -= 1''' size = size * 1000 for x in range(0, 4): d.driveTimed(80, 80, size) msleep(200) d.rotate(90) msleep(200)
def wiggle(): print("wiggle") moveClaw(c.clawWiggle, 15) driveTimed(100, 0, 150) driveTimed(0, 100, 250) driveTimed(100, 0, 150) driveTimed(0, 100, 250) #driveTimed(100, 0, 100) #driveTimed(0, 100, 100) #driveTimed(0, 100, 100) moveClaw(c.clawClose, 5) # moveClaw(c.clawOpen, 20) # moveArm(c.armFront, 20) # msleep(500) # driveTimed(100, 100, 1000) # msleep(1000) # moveClaw(c.clawClose, 20) # msleep(500) # driveTimed(-100, -100, 250) # if (atTest): # print "I SEE SOMETHING!" # moveClaw(c.clawMid, 20) # msleep(500) # driveTimed(100, 100, 500) # moveClaw(c.clawClose, 20) # msleep(500)
def lineUpWithRamp(): print("lineUpWithRamp") driveTimed(100, 20, 2000) driveTimed (0, 100, 1000) driveTimed(-100, -80, 4000) drive(-100, -20) moveOutrigger(c.outriggerBin, 25) msleep(1750) driveTimed(-100, -100, 3000)
def backUpFromBin(): print("backUpFromBin") if c.isPrime: #added at practice driveTimed(-100, -100, 250) else: pass driveTimed(-100, -50, 2100) driveTimed(100, 20, 750) if c.isPrime: driveTimed(100, 100, 250) # added at practice else: pass
def goToCenterAgain(): print("goCenterAgain") driveTimed(95, 100, 3000) DEBUG() driveTimed(100, 60, 3000) drive(100, 100) while not onBlack(c.LINE_FOLLOWER): pass drive(100, 0) while onBlack(c.LINE_FOLLOWER): pass stop() timedLineFollowRight(c.LINE_FOLLOWER, 2) drive(100, 100) moveClaw(c.clawOpen, 25) lineFollowUntilEndRight(c.LINE_FOLLOWER) driveTimed(100, 0, 150) msleep(400) driveTimed(100, 100, 1500)
def goToWestPile(): print("goToWestPile") moveClaw(c.clawOpen, 20) msleep(300) driveTimed(95, 100, 500) moveClaw(c.clawClose, 15) msleep(300) drive(95, 100) msleep(1000) moveArm(c.armBump, 20) msleep(500) print("armBump") driveTimed(95,100, 1250) moveArm(c.armFront,10) moveClaw(c.clawWiggle, 20) #clawOpen driveTimed(95, 100, 3000)
def goToNorthernPile(): print("goToNorthernPile") moveClaw(c.clawOpen, 30) moveArm(c.armMid, 20) drive(100, 90) while not onBlack(c.STARBOARD_TOPHAT): pass driveTimed(100, 100, 150) moveOutrigger(c.outriggerTurn, 20) drive(100, -20) #was drive(100, -20) while not onBlack(c.STARBOARD_TOPHAT): pass moveArm(c.armFront, 10) if c.isClone: driveTimed(0, 100, 200) #300 stop() driveTimed(0, 100, 300)
def init(): #Prime Setup: #The square up surface on the back of the bot should be flush to the back of the SB #The left edge of the square up surface should be just to the left of the coupler #Just use the marks on the table :) if c.is_clone: print("Hi! I'm Clone.") else: print("Hi! I'm Prime.") enable_servos() msleep(500) print("Don't touch me, I'm calibrating!!!") g.calibrate_gyro() msleep(500) u.move_servo(c.servo_arm, c.arm_drop_off, 10) # test the motors d.driveTimed(50, 50, 1000) d.driveTimed(50, 0, 1000) d.driveTimed(-50, 0, 1000) print("testing wrist") u.move_servo(c.servo_wrist, c.wrist_vertical) if c.is_prime: msleep(300) u.move_servo(c.servo_wrist, c.wristFlipped) msleep(300) u.move_servo(c.servo_wrist, c.wrist_horizontal) print("testing claw") u.move_servo(c.servo_claw, c.claw_closed) u.move_servo(c.servo_claw, c.claw_open) print("testing tophat") g.drive_condition(50, d.on_black_left, False) g.drive_condition(50, d.on_black_right, True) d.drive_to_white_and_square_up(50) print("testing arm") u.move_servo(c.servo_arm, c.arm_up, 10) u.move_servo(c.servo_arm, c.arm_down, 5) print("place in start posistion") u.waitForButton() g.calibrate_gyro()
def returnToBase(): print ("returntobase") moveArm(c.armBlockBack, 10) driveTimed(-100, 0, 1000) drive(-100, -87) #-85 while not onBlack(c.STARBOARD_TOPHAT): pass driveTimed(-100, 0, 300) timedLineFollowBack(c.STARBOARD_TOPHAT, 3)#2 driveTimed(-80, -100, 1000) drive(-90, -100) msleep(3500); moveOutrigger(c.outriggerBaseReturn, 20) msleep(1000) while(onBlack(c.STARBOARD_TOPHAT)): pass msleep(10) while(onBlack(c.STARBOARD_TOPHAT)): pass msleep(10) while(onBlack(c.STARBOARD_TOPHAT)): pass stop()
def recollectNorthPile(): driveTimed(-90, -100, 500) msleep(300) moveClaw(c.clawWiggle, 10) driveTimed(90, 100, 700) moveClaw(c.clawClose, 10)
def smoothLineFollowLeftCondition(speed): #Proportional adjustment LF sec = seconds() + 7 while w.analog(c.LEFT_TOPHAT) < 2000 and seconds() < sec: num = ((w.analog(4) - 1500) / 75) d.driveTimed(speed - num, speed + num, 10)
def goToCenter(): print("goToCenter") if c.isPrime: driveTimed(95, 100, 4000) driveTimed(100, 60, 3000) #3000 else: driveTimed(90, 100, 4000) driveTimed(100, 60, 3000) drive(100, 100) while not onBlack(c.LINE_FOLLOWER): pass drive(100, 0) while onBlack(c.LINE_FOLLOWER): pass stop() timedLineFollowRight(c.LINE_FOLLOWER, 1) drive(100, 100) moveClaw(c.clawOpen, 25) lineFollowUntilEndRight(c.LINE_FOLLOWER) driveTimed(90, 50, 200) moveArm(c.armShovel, 15) msleep(400) driveTimed(100, 100, 1500)