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)
