def calibrate(port):
print "Press LEFT button with light on"
while not left_button():
pass
lightOn = analog(port)
print "On value =", lightOn
if lightOn > 200:
print "Bad calibration"
return False
msleep(1000)
print "Press RIGHT button with light off"
while not right_button():
pass
lightOff = analog(port)
print "Off value =", lightOff
if lightOff < 3000:
print "Bad calibration"
return False
if (lightOff - lightOn) < 2000:
print "Bad calibration"
return False
c.startLightThresh = (lightOff - lightOn) / 2
print "Good calibration! ", c.startLightThresh
return True
def waitForButton():
print("Press Right Button...")
while not digital(c.RIGHT_BUTTON):
pass
msleep(1)
print("Pressed")
msleep(1000)
def waitForButton():
print "Press Button..."
while not digital(c.RIGHT_BUTTON):
pass
msleep(1)
print "Pressed"
msleep(1000)
def rotate_spinner(rotations, speed):
full_rotation = 1400.0
start = get_motor_position_counter(SPINNER)
motor_power(SPINNER, speed)
tries_remaining = 3
previous = 0
counter = 0
while abs(get_motor_position_counter(SPINNER) - start) < abs(
full_rotation * rotations) and tries_remaining > 0:
if counter >= 10:
counter = 0
if tries_remaining > 0:
motor_power(SPINNER, int(-speed))
msleep(300)
motor_power(SPINNER, speed)
tries_remaining -= 1
elif abs(get_motor_position_counter(SPINNER)) == previous:
counter += 1
else:
counter = 0
previous = abs(get_motor_position_counter(SPINNER))
msleep(10)
print "rotated {} out of {}".format(
get_motor_position_counter(SPINNER) - start,
abs(full_rotation * rotations))
freeze(SPINNER)
def waitForButton():
print "Press Button..."
while not digital(c.RIGHT_BUTTON):
pass
msleep(1)
print "Pressed"
msleep(1000)
def calibrate(port):
print("Press LEFT button with light on")
while not w.left_button():
pass
while w.left_button():
pass
lightOn = w.analog(port)
print("On value =", lightOn)
if lightOn > 200:
print("Bad calibration")
return False
msleep(1000)
print("Press RIGHT button with light off")
while not w.right_button():
pass
while w.right_button():
pass
lightOff = w.analog(port)
print("Off value =", lightOff)
if lightOff < 3000:
print("Bad calibration")
return False
if (lightOff - lightOn) < 2000:
print("Bad calibration")
return False
c.startLightThresh = (lightOff - lightOn) / 2
print("Good calibration! ", c.startLightThresh)
return True
def arc_radius(angle, turnRadius, speed): # Turns the robot "angle" degrees by arcing about "turnRadius".
smallCircRadius = turnRadius - (WHEEL_DISTANCE / 2)
largeCircRadius = turnRadius + (WHEEL_DISTANCE / 2)
smallCircum = pi * 2 * smallCircRadius
largeCircum = pi * 2 * largeCircRadius
smallCircSeg = (angle / 360.0) * smallCircum
largeCircSeg = (angle / 360.0) * largeCircum
if turnRadius < 0: msleep(300)
pivot_right(3, -5)
speed = -speed
_clear_ticks()
smallTicks = abs(INCHES_TO_TICKS * smallCircSeg)
largeTicks = abs(INCHES_TO_TICKS * largeCircSeg)
if angle > 0:
smallSpeed = int(speed * (smallTicks / largeTicks))
largeSpeed = int(speed)
print smallTicks
print largeTicks
print smallTicks / largeTicks
print smallSpeed
print largeSpeed
while _right_ticks() <= largeTicks:
if (_right_ticks() / largeTicks) == (_left_ticks() / smallTicks):
_drive(smallSpeed, largeSpeed)
if (_right_ticks() / largeTicks) > (_left_ticks() / smallTicks):
_drive(smallSpeed, int(largeSpeed / 1.3))
if (_left_ticks() / smallTicks) > (_right_ticks() / largeTicks):
_drive(int(smallSpeed / 1.3), largeSpeed)
freeze_motors()
print smallTicks
print largeTicks
print get_motor_position_counter(RMOTOR)
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 calibrate(port):
display("Press LEFT button with light on")
while not left_button():
pass
while left_button():
pass
lightOn = analog(port)
display("On value =" + str(lightOn))
if lightOn > 200:
display("Bad calibration")
return False
msleep(1000)
display("Press RIGHT button with light off")
while not right_button():
pass
while right_button():
pass
lightOff = analog(port)
display("Off value =" + str(lightOff))
if lightOff < 3000:
display("Bad calibration")
return False
if (lightOff - lightOn) < 2000:
display("Bad calibration")
return False
c.startLightThresh = (lightOff - lightOn) / 2
display("Good calibration! " + str(c.startLightThresh))
return True
def wait_for_button():
display("Press Button...")
while not digital(c.RIGHT_BUTTON):
pass
msleep(1)
display("Pressed")
msleep(1000)
def grabWestPile():
print("grabWestPile")
#drive(95, 100)
moveClaw(c.clawMid, 10)
moveClaw(c.clawClose, 15)
#moveArm(c.armMid, 15)
msleep(500)
def ETLineFollowRight(port, findCeiling):
while atArmLength() ^ findCeiling :
if not onBlack(port):
driveTimed(50, 100, 20)
else:
driveTimed(100, 50, 20)
msleep(10)
def calibrate(port):
print("Press LEFT button with light on")
while not w.left_button():
pass
while w.left_button():
pass
lightOn = w.analog(port)
print("On value =", lightOn)
if lightOn > 4000:
print("Bad calibration")
return False
w.msleep(1000)
print("Press RIGHT button with light off")
while not w.right_button():
pass
while w.right_button():
pass
lightOff = w.analog(port)
print("Off value =", lightOff)
if lightOff < 1000:
print("Bad calibration")
return False
if (lightOff - lightOn) < 1:
print("Bad calibration")
return False
startLightThresh = (lightOff + lightOn) / 2
print("Good calibration! ", startLightThresh)
print('{} {} {}'.format(lightOff, lightOn, startLightThresh))
return startLightThresh
def DEBUGwithWait():
freeze(c.LMOTOR)
freeze(c.RMOTOR)
#print ('Program stop for DEBUG\nSeconds: ', seconds() - startTime)
ao()
msleep(5000)
exit(0)
def DEBUG_WITH_WAIT():
freeze(c.LMOTOR)
freeze(c.RMOTOR)
ao()
msleep(5000)
display('Program stop for DEBUG\nSeconds: {}'.format(seconds() -
c.startTime))
exit(0)
def timedLineFollowLeftBack(time): # follows on starboard side
sec = seconds() + time
while seconds() < sec :
if onBlackBack():
driveTimed(-90, -20, 20)
else:
driveTimed(-20, -90, 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 timedLineFollowLeft(time):
sec = seconds() + time
while seconds() < sec :
if onBlackFront():
driveTimed(20, 90, 20)
else:
driveTimed(90, 20, 20)
msleep(10)
def attempt():
enable_servos()
for x in range (0, 60):
if analog (4) > 100:
print "i see something"
else:
print "i don't see anything"
msleep(1000)
def timedLineFollowLeftSmooth(port, time):
sec = seconds() + time
while seconds() < sec:
if onBlack(port):
driveTimed(20, 40, 20)
else:
driveTimed(40, 20, 20)
msleep(10)
def timedLineFollowBack(port, time):
sec = seconds() + time
while seconds() < sec:
if onBlack(port):
driveTimed(-90, -20, 20)
else:
driveTimed(-20, -90, 20)
msleep(10)
def smoothLineFollowLeft(time, speed):
#Max speed is 80
#Proportional adjustment LF
sec = seconds() + time
while seconds() < sec:
num = ((w.analog(5) - 1500) / 120)
d.driveTimed(speed-num, speed+num, 20)
msleep(10)
def timedLineFollowRight(port, time):
sec = seconds() + time
while seconds() < sec:
if not onBlack(port):
driveTimed(20, 90, 20)
else:
driveTimed(90, 20, 20)
msleep(10)
def timedLineFollowRightSmooth(time):
sec = seconds() + time
while seconds() < sec:
if not onBlackFront():
driveTimed(20, 40, 20)
else:
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 timedLineFollowLeftSmooth(time):
sec = seconds() + time
while seconds() < sec:
if onBlackFront():
d.driveTimed(20, 40, 20)
else:
d.driveTimed(40, 20, 20)
msleep(10)
def calibrate():
i = 0
avg = 0
while i < 50:
avg += w.Gyro.z()
w.msleep(1)
i += 1
c.bias = avg / i
w.msleep(60)
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 _stepped_servo_position_change(self, target_position, steps, time_for_change):
if((self.servo_min <= target_position) and (self.servo_max >= target_position)):
time_per_cycle = float(time_for_change)/float(steps) # optimize
pos_change_per_cycle = (target_position-self.current_servo_position)/steps
for i in range(steps):
self.current_servo_position=self.current_servo_position+ pos_change_per_cycle
self._set_servo_pos(self.current_servo_position)
wallaby.msleep(time_per_cycle)
self._set_servo_pos(target_position)
else:
raise Exeception("Attempt to set servo to an out of bound position. attempted to set value to : " + str(target_position))
def main():
r = roomba(True)
wallaby.msleep(1000)
r.drive(100,100)
wallaby.msleep(1000)
r.drive(300,300)
wallaby.msleep(1000)
r.stop()
wallaby.msleep(1000)
r.drive(100, -100)
wallaby.msleep(1000)
def goToCow():
msleep(300)
#u.move_servo(c.servoCow, c.cowUp, 10)
msleep(300)
x.drive_speed(7, -50)
x.rotate(-90, 20)
x.drive_speed(14, 50)
x.rotate(185, 20)
x.drive_speed(2, 50)
u.DEBUG()
x.drive_speed(9, -50)
def tempInit():
c.startTime = seconds()
tempServos()
moveOutrigger(c.outriggerIn, 100)
moveArm(c.armUp, 10)
msleep(500)
moveClaw(c.clawMid, 15)
msleep(500)
binGrabUp()
waitForButton()
c.startTime = seconds()
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 grabMiddlePile():
print("grabMiddlePile")
moveClaw(c.clawOpen, 25)
moveArm(c.armFront, 25)
drive(100, 100)
msleep(300)
timedLineFollowLeft(c.STARBOARD_TOPHAT, 3)
moveClaw(c.clawMid, 10)
drive(60, 60)
moveClaw(c.clawClose, 5)
moveArm(c.armMid, 25)
stop()
deliverPoms()
def calibrate5():
_clear_ticks()
while not digital(13):
try:
print str(_left_ticks()) + " " + str(_right_ticks()) + " " + str(
_right_ticks() / _left_ticks())
except Exception:
pass
msleep(100)
x = _right_ticks() / _left_ticks()
global lAdjust
lAdjust = x
print(x)
def rotate_until_stalled(speed):
counter = 0
motor_power(SPINNER, speed)
previous = abs(get_motor_position_counter(SPINNER))
while counter < 10:
if abs(get_motor_position_counter(SPINNER)) == previous:
counter += 1
else:
counter = 0
previous = abs(get_motor_position_counter(SPINNER))
msleep(10)
freeze(SPINNER)
clear_motor_position_counter(SPINNER)
def grabSouthPile():
print ("grabSouthPile")
moveClaw(c.clawOpen, 10)
moveArm(c.armFront, 15)
moveArm(c.armShovel, 10)
timedLineFollowLeft(c.STARBOARD_TOPHAT, 5)
drive(50, 50) #now same on both
moveArm(c.armFront, 50)
moveClaw(c.clawClose, 5)
msleep(500)
stop()
moveArm(c.armMid, 15)
deliverPoms()
moveOutrigger(c.outriggerFindLine, 25)
def move_to_med(): # move to medical center
print "Moving towards the %s medical center" % (
("close", "far")[c.burning_center])
d.degreeTurn(50, ((-80, -70)[c.last_direction],
(-95, -85)[c.last_direction])[c.burning_center])
d.skipLine(50, c.largeTopHat.port(), c.LARGE_TOPHAT_LINE,
(1, 3)[c.burning_center])
print "skipped the line!"
d.forward(50, 1000)
print "moving forward!"
w.ao()
w.msleep(100)
d.backward(50, 2000)
print "moving backward!"
def testMotors():
drive(100, 100)
while not onBlack(c.LINE_FOLLOWER): # wait to see line
pass
stop()
drive(75, 0)
while not onBlack(c.STARBOARD_TOPHAT):
pass
stop()
drive(-75, 0)
while not onBlack(c.LINE_FOLLOWER):
pass
msleep(100)
stop()
def rotate_compass(deg, speed): # Rotates by using both wheels equally.
if deg < 0:
speed = -speed
deg = -deg
angle = deg / 360.0
circ = pi * WHEEL_DISTANCE
inches = angle * circ
print circ
print inches
ticks = int(INCHES_TO_TICKS * inches)
_clear_ticks()
_drive(-speed, speed)
for _ in range(0, 10):
magneto_x()
magneto_y()
msleep(10)
mx = magneto_x()
my = magneto_y()
while abs(mx) > 100 or abs(my) > 100:
mx = magneto_x()
my = magneto_y()
data = {"min_x": mx, "min_y": my, "max_x": mx, "max_y": my}
while _right_ticks() <= ticks:
mag_x = magneto_x()
mag_y = magneto_y()
print(str(mag_x) + "\t" + str(mag_y))
if mag_x > data["max_x"] and abs(
mag_x -
data["max_x"]) < 5: # and abs(mag_x - data["max_x"])< 10
data["max_x"] = mag_x
# print("new max")
if mag_y > data["max_y"] and abs(mag_y - data["max_y"]) < 5:
data["max_y"] = mag_y
print("new max y " + str(mag_y))
if mag_x < data["min_x"] and abs(mag_x - data["min_x"]) < 5:
data["min_x"] = mag_x
if mag_y < data["min_y"] and abs(mag_y - data["min_y"]) < 5:
data["min_y"] = mag_y
freeze_motors()
print get_motor_position_counter(RMOTOR)
return data
def testMotors():
drive(-100, -100)
while not onBlackFront(): # wait to see line
pass
stop()
drive(100, 100)
while not onBlackBack(): # wait to see line
pass
stop()
driveTimed(-70, 0, 1000)
driveTimed(70, 0, 1200)
msleep(1000)
def calibrate(dist=0, num=0): # WIP: Used to calibrate the constants for this module. Run as "calibrate()" to begin.
if dist is 0:
_clear_ticks()
_drive(30, 30)
msleep(3000)
freeze_motors()
print "Run the calibrate method again, but pass the distance traveled (inch) and the following number in:"
print (_right_ticks() + _left_ticks()) / 2
elif num is 0:
drive_speed(6, 30)
print "did it go " + str(dist) + " inches? If not, make slight adjustments to INCHES_TO_TICKS until it does."
else:
print "enter " + str(int(num / dist)) + " as INCHES_TO_TICKS. run calibrate again, but as calibrate(" + str(
dist) + ", 0)"
exit(0)
def move_two_servos_timed(servo1, endPos1, servo2, endPos2, time):
if time == 0:
speed = 2047
else:
delta1 = endPos1 - get_servo_position(servo1)
delta2 = endPos2 - get_servo_position(servo2)
speed1 = (DELAY * delta1) / time
speed2 = (DELAY * delta2) / time
moves = delta1 / speed1
for x in range(1, abs(int(moves))):
set_servo_position(servo1, get_servo_position(servo1) + speed1 * x)
set_servo_position(servo2, get_servo_position(servo2) + speed2 * x)
msleep(DELAY)
set_servo_position(servo1, endPos1)
set_servo_position(servo2, endPos2)
def drive_with_gyro(speed, time):
theta = 0
start_time = w.seconds()
while w.seconds - start_time < (time / 1000.0):
if speed > 0:
c.leftMotor.moveAtVelocity(
int(speed - speed * (1.920137e-16 + 0.000004470956 * theta)))
c.rightMotor.moveAtVelocity(
int(speed + speed * (1.920137e-16 + 0.000004470956 * theta)))
else:
c.leftMotor.moveAtVelocity(
int(speed + speed * (1.920137e-16 + 0.000004470956 * theta)))
c.rightMotor.moveAtVelocity(
int(speed - speed * (1.920137e-16 + 0.000004470956 * theta)))
w.msleep(10)
theta += (w.gyro_z() - bias) * 10
def moveServo(servo, endPos, speed=10):
# speed of 1 is slow
# speed of 2000 is fast
# speed of 10 is the default
now = get_servo_position(servo)
if now > 2048 :
PROGRAMMER_ERROR ("Servo setting too large")
if now < 0 :
PROGRAMMER_ERROR ("Servo setting too small")
if now > endPos:
speed = -speed
for i in range (now, endPos, speed):
set_servo_position(servo, i)
msleep(10)
set_servo_position(servo, endPos)
msleep(10)
def upRamp():
x.rotate(25, 50)
x.drive_speed(36, 100)
x.drive_speed(6, -50)
x.rotate(89, 50)
x.drive_speed(30, 100)
x.drive_speed(4, -50)
x.rotate(79, 50)
u.move_servo(c.servoArm, c.armBotguy,10)
msleep(300)
x.drive_speed(70, 75)
u.move_servo(c.servoCow, c.cowDown, 10)
msleep(300)
x.rotate(-45, 50)
print "Seconds elapsed: " + seconds() - c.startTime
def calibrate6():
global lAdjust
lAdjust = 1
while True:
drive_speed(3, 50)
if analog(0) > 1500:
lAdjust = lAdjust + 0.05
while not digital(13):
pass
msleep(500)
elif analog(0) < 1000:
lAdjust = lAdjust - 0.05
while not digital(13):
pass
msleep(500)
print lAdjust
def driveMotor(left, right, time): w.motor(c.leftMotor, int(left*c.motorScale)) w.motor(c.rightMotor, right) if time == 0: w.msleep(time) w.off(c.leftMotor) w.off(c.rightMotor) def forward(speed, time): driveMotor(speed, speed, time) def backward(speed, time): driveMotor(speed*-1, speed*-1, time) def spinLeft(speed, time): driveMotor(speed*-1, speed, time) def spinRight(speed, time): driveMotor(speed, speed*-1, time) def veerLeft(speed, time, o): driveMotor((speed*-1)-o, speed, time) def veerRight(speed, time, o): driveMotor(speed, (speed*-1)-o, time) def pivotRight(speed, time): driveMotor(0, speed, time) def pivotLeft(speed, time): driveMotor(speed, 0, time) def radiusTurn(speed, radius, time): driveMotor(speed, int((radius / (radius+5.0)*speed), time)) def driveUntilBlack(speed): while w.analog(c.largeTopHat) < c.LARGE_TOPHAT_LINE: driveMotor(speed, speed, 1) def lineFollowUntilTape(): while w.analog(c.smallTopHat) < c.SMALL_TOPHAT_LINE: if w.analog(c.largeTopHat) < c.LARGE_TOPHAT_LINE: veerLeft(50, 1, 10) elif w.analog(c.largeTopHat) > c.LARGE_TOPHAT_LINE: veerRight(50, 1, 10)
def calibrate3():
AMOUNT = 6000
_clear_ticks()
_drive(50, 50)
startR = seconds()
while _right_ticks() < AMOUNT:
pass
endR = seconds() - startR
freeze_motors()
msleep(5000)
_clear_ticks()
_drive(50, 50)
startL = seconds()
while _left_ticks() < AMOUNT:
pass
endL = seconds() - startL
print "Value: " + str(endL / endR)
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 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)
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 testET():
print("Put your hand in front of ET")
i = 0
while getET() >= 2000:
print "BLOCKED!"
msleep(333)
binGrabDown()
msleep(300)
binGrabUp()
msleep(300)
while getET() < 2000:
if i > 0:
binGrabUp()
i = 0
else:
binGrabDown()
i = 1
msleep(300)
binGrabDown()
driveTimed(-100, -100, 1000)
stop()
def grabNorthPile():
print("grabNorthPile")
drive(93, 100)#90
moveClaw(c.clawMid, 10)
msleep(1000)
drive(54, 50)#45,50
moveClaw(c.clawClose, 4)
msleep(500)
stop()
#moveArm(c.armMid, 15)
#msleep(500)
moveClaw(c.clawOpen, 10)
msleep(500)
moveClaw(c.clawClose, 10)
'''driveTimed(-50, -50, 1000)
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 testServos():
print "Testing servos"
set_servo_position(c.ARM, c.armUp)
set_servo_position(c.CLAW, c.clawClose)
set_servo_position(c.OUTRIGGER, c.outriggerIn)
enable_servos()
msleep(1000)
moveClaw(c.clawOpen, 25)
msleep(500)
moveClaw(c.clawClose, 25)
msleep(500)
moveArm(c.armBack, 15)
msleep(500)
moveOutrigger(c.outriggerOut, 15)
msleep(500)
moveArm(c.armFront, 15)
moveClaw(c.clawMid, 25)
msleep(500)
def depositWestPile():
print("depositWestPile")
moveArm(c.armMid, 5)
moveClaw(c.clawMid, 10)
msleep(400)
moveClaw(c.clawClose, 15)
def recollectNorthPile():
driveTimed(-90, -100, 500)
msleep(300)
moveClaw(c.clawWiggle, 10)
driveTimed(90, 100, 700)
moveClaw(c.clawClose, 10)
def deliverPoms():
moveArm(c.armBack, 25)
msleep(500)
moveClaw(c.clawMid, 25)