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)