def grabcan():
set_servo_position(c.clawservo, 1775)
msleep(1000)
m.drive(10,50,400)
set_servo_position(c.clawservo, 800)
set_servo_position(c.armservo, 390)
msleep(1000)
wait_for_button()
def lineFollowToCan():
m.drive(100, 100, 100)
#follow line until It gets close to the can
dist = analog(c.etPort)
while(dist<2600):
dist=analog(c.etPort)
print analog(c.thport)
m.linefollow()
ao()
def main():
a.init()
a.lineFollowToCan()
a.wait_for_button()
a.grabcan()
a.wait_for_button()
for x in range(0, 4):
m.drive(100, 100, 1000)
m.drive(100, 0, 980)
set_servo_position(c.clawservo,
2047) #opens up claw all the way to grab can
msleep(1000)
# Stop
ao()
msleep(1000)
