def setupLineFollow(): d.driveUntilBlack(100) while w.analog(c.largeTopHat) > c.LARGE_TOPHAT_LINE: d.forward(100, 1) while w.analog(c.largeTopHat) < c.LARGE_TOPHAT_LINE: d.spinLeft(100, 1) d.lineFollowUntilTape()
def shake_down():
c.collection_arm.enable()
c.collection_arm.setPosition(1550)
w.msleep(1500)
d.forward(50, 1000)
print "moving forward"
w.msleep(500)
print "moving backward"
d.backward(50, 1000)
w.msleep(500)
moveDegree(c.spinner.port(), 50, 90)
print "spinning +90 degrees"
w.msleep(500)
moveDegree(c.spinner.port(), 50, -90)
print "spinning -90 degrees"
w.msleep(500)
c.camera_servo.enable()
c.camera_servo.setPosition(2047)
print "-90 servo"
w.msleep(500)
c.camera_servo.setPosition(900)
print "+90 servo"
w.msleep(500)
c.camera_servo.setPosition(0)
print "+90 servo"
w.msleep(500)
d.driveUntilBlack(50)
print "checking tophat!"
w.msleep(500)
print "testing the camera and burning centers!"
w.console_clear()
print "Not Ready! Awaiting input!"
while not w.left_button():
pass
def movement(data):
log.debug("======MOVEMENT======")
log.debug(data)
# +x = right
# -x = left
# +y = forward
# -y = reverse
# y==0 -> neutral
# x==0 -> straight
x = int(data['x'])
y = int(data['y'])
if (y > 0):
drive.forward(y)
elif (y < 0):
drive.reverse(y)
else:
drive.neutral()
if (x > 0):
drive.right(x * (-1)) # drive API is right (-)
elif (x < 0):
drive.left(x * (-1))
else:
drive.straight()
# call python to adjust PWM
return "OK"
def move_out_startbox():
c.camera_servo.enable()
c.camera_servo.setPosition(
1400) # set the servo so It can see both centers, but not cubes
c.collection_arm.setPosition(270) # set the arm to so it is open
d.forward(50, 650)
d.spinLeft(50, 100)
def move(self):
if (self.direction == 'backwards'):
if (self.requested_speed > 0):
drive.reverse(self.requested_speed)
else:
drive.stop()
else:
if (self.requested_speed > 0):
drive.forward(self.requested_speed)
else:
drive.stop()
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 burning_center_test():
c.camera_servo.enable()
c.camera_servo.setPosition(
1400) # set the servo so It can see both centers, but not cubes
c.collection_arm.setPosition(270) # set the arm to so it is open
d.forward(50, 650)
d.spinLeft(50, 100)
if not w.camera_open():
return
while not w.right_button():
w.camera_update()
best = x.getGreatest(c.BURNING)
if w.get_object_confidence(c.BURNING, best) < 0.2:
continue
x_pos = w.get_object_center_x(c.BURNING, best)
print x_pos
if 0 < x_pos < 79:
print "Close medical center"
break
elif 80 < x_pos < 140:
print "Far medical center"
break
def turn_to_cubes():
if not w.camera_open():
return
servo_centered = False
c.camera_servo.enable()
while not servo_centered: # first center the
w.camera_update()
objects = w.get_object_count(c.YELLOW)
if objects == 0:
print "no objects!"
c.camera_servo.setPosition(2047)
continue
best = x.getGreatest(c.YELLOW)
if w.get_object_confidence(c.YELLOW, best) < 0.5:
continue
print "Cube:",
print w.get_object_center_x(c.YELLOW, best),
print w.get_object_confidence(c.YELLOW, best)
x.centerX_servo(c.YELLOW, best, 10, 50) # first find it with our servo
if (w.get_camera_width() / 2) - 25 < w.get_object_center_x(
c.YELLOW, best) < (w.get_camera_width() / 2) + 10:
servo_centered = True
break
servo_pos = c.camera_servo.position()
print servo_pos
print c.SERVO_TICK2DEG(servo_pos)
print int(c.SERVO_TICK2DEG(servo_pos))
print int((c.SERVO_TICK2DEG(servo_pos)) - 90)
d.forward(50, 2100)
d.stop()
w.msleep(250)
d.degreeTurn(50, int(c.SERVO_TICK2DEG(servo_pos) - 75))
c.camera_servo.setPosition(900) # reset camera to default position
u.moveDegree(c.spinner.port(), 50, 90) # set our sweeper to default pos
c.distance_traveled = 0 # clear distance
w.camera_close()
import RPi.GPIO as GPIO import time import drive,motor_init GPIO.setmode(GPIO.BCM) motor_init.initialize() drive.forward() time.sleep(2) drive.backward()
if decision == "stay":
drive.stop()
elif decision == "left":
drive.turn_left(0.01)
elif decision == "right":
drive.turn_right(0.01)
else:
drive.stop()
if distance_prediction != "No object":
if abs(distance_prediction -
DESIRED_DISTANCE) > DESIRED_DISTANCE_TOLERANCE:
if distance_prediction - DESIRED_DISTANCE < 0:
drive.backward(0.015)
else:
drive.forward(0.015)
else:
drive.stop()
# Break on key pressed
if select.select([
sys.stdin,
], [], [], 0.0)[0]:
break
# Release camera
time.sleep(0.5)
cap.release()
GPIO.cleanup()
time.sleep(1)
def exitStart(): d.forward(100, 1000) d.spinLeft(100, 600) d.forward(100, 4000)
g.output(37, False)
g.output(36, False)
g.output(38, False)
while True:
if g.input(29) == True:
drive.turn_right(0.5, 35, 36, 37, 38)
if g.input(31) == True:
drive.turn_left(0.5, 35, 36, 37, 38)
if g.input(15) == True:
drive.turn_right(1, 35, 36, 37, 38)
if g.input(33) == True:
drive.turn_left(1, 35, 36, 37, 38)
if g.input(29) == False:
drive.forward(1, 35, 36, 37, 38)
if g.input(31) == False:
drive.forward(1, 35, 36, 37, 38)
if g.input(15) == False:
drive.forward(1, 35, 36, 37, 38)
if g.input(33) == False:
drive.forward(1, 35, 36, 37, 38)
g.cleanup()
def move_to_cubes():
c.camera_servo.setPosition(900)
d.forward(50, 500)
w.msleep(250)
d.degreeTurn(50, 70)
u.moveDegree(c.spinner.port(), 50, 90)
c.distance_traveled = 0 # reset distance
if not w.camera_open():
return
at_cubes = False
ol = -1
while not at_cubes:
w.camera_update()
objects = w.get_object_count(c.YELLOW)
if objects == 0:
print "no objects!"
c.can_see = False
if ol == 2:
if c.last_direction == 0:
if c.last_seen_x < w.get_camera_width() + 5:
d.spinLeft(10, 200)
d.forward(10, 200)
else:
if c.last_seen_x < w.get_camera_width() - 5:
d.spinRight(10, 200)
d.forward(10, 200)
w.ao()
at_cubes = True
print "No objects, but passed line!"
break
continue
c.can_see = True
best = x.getGreatest(c.YELLOW)
c.last_seen_x = w.get_object_center_x(c.YELLOW, best)
x.centerX(c.YELLOW, best)
print ol,
if u.isOnLine(c.largeTopHat.port(), c.LARGE_TOPHAT_LINE) and ol == -1:
ol += 1
print "Hitting first line"
elif not u.isOnLine(c.largeTopHat.port(),
c.LARGE_TOPHAT_LINE) and ol == 0:
ol += 1
print "Passed First line"
elif u.isOnLine(c.largeTopHat.port(), c.LARGE_TOPHAT_LINE) and ol == 1:
d.stop()
c.collection_arm.setPosition(1600)
w.msleep(500)
c.collection_arm.setPosition(270)
w.msleep(500)
ol += 1
print "Hitting second line"
elif not u.isOnLine(c.largeTopHat.port(),
c.LARGE_TOPHAT_LINE) and ol == 2:
d.stop()
at_cubes = True
if c.last_direction == 0:
if c.last_seen_x < w.get_camera_width() + 10:
d.spinLeft(10, 200)
d.forward(10, 200)
else:
if c.last_seen_x < w.get_camera_width() - 10:
d.spinRight(10, 200)
d.forward(10, 200)
print "Passed second line"
break
print "Cube:",
print w.get_object_center_x(c.YELLOW, best),
print w.get_object_confidence(c.YELLOW, best)
w.camera_close()
def return_to_med():
print "Moving back towards the medical center!"
for _ in range(0, 2):
d.forward(50, 1000)
w.msleep(500)
d.backward(50, 1000)
