def deliverBin(): print("deliverBin") drive(-100, 0) while not onBlack(c.STARBOARD_TOPHAT): pass stop() driveTimed(100, 100, 500)
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 arc_right(dist, power=100): """ Sweeping turn to the right :param dist: :param power: """ leftWheel.reset() rightWheel.reset() while leftWheel.count < dist: drive.move(power, power - 20) drive.stop()
def flagGrab(): drive.moveBackward() time.sleep(1) drive.stop() drive.turnLeft90() drive.turnLeft90() release() drive.moveBackward() time.sleep(1) drive.stop() grab()
def goToTaterBin(): print("goToTaterBin") moveArm(c.armMid, 10) drive(95, 100) moveOutrigger(c.outriggerApproach, 20) while not onBlack(c.STARBOARD_TOPHAT): pass drive(25, 30) while not atArmLength(): pass stop()
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 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 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 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 align_encoder(): """ Ensure both wheels are aligned on encoders """ leftWheel.reset() while leftWheel.count < 1: drive.move(40, 0) drive.stop() rightWheel.reset() while rightWheel.count < 1: drive.move(0, 40) drive.stop()
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 main(): while True: try: uValueC = BP.get_sensor(BP.PORT_2) except brickpi3.SensorError as error: print(error) if(uValueC == 0): drive.stop() else: if(uValueC <= 5): drive.turnLeft90() checkObject() else: drive.moveForward() time.sleep(0.02)
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 avoidance(): uValueC = 70 try: uValueC = BP.get_sensor(BP.PORT_2) except brickpi3.SensorError as error: print(error) if (uValueC == 0): drive.stop() return False else: if (uValueC <= 5): drive.turnLeft90() return True else: drive.moveForward() return False
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 reverse_dist(dist, power): """ Travel a given distance in reverse :param dist: :param power: """ leftWheel.reset() rightWheel.reset() while leftWheel.count < dist or rightWheel.count < dist: log("leftRev={0} vs rightRev={1}".format(leftWheel.count, rightWheel.count)) if leftWheel.count < rightWheel.count: log("Teak right") drive.move(-power, 0) elif leftWheel.count > rightWheel.count: log("Teak left") drive.move(0, -power) else: log("No tweaks") drive.move(-power, -power) drive.stop()
def forward_dist(dist, power=100): """ Travel forwards a given distance :param dist: :param power: """ leftWheel.reset() rightWheel.reset() while leftWheel.count < dist or rightWheel.count < dist: log("leftFwd={0} vs rightFwd={1}".format(leftWheel.count, rightWheel.count)) if leftWheel.count < rightWheel.count: log("Teak right") drive.move(power, 0) elif leftWheel.count > rightWheel.count: log("Teak left") drive.move(0, power) else: log("No tweaks") drive.move(power, power) drive.stop() log("Distance reached so stopping")
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 avoidance(self): uValue = 70 uValue = self.getUltrasonic() if (uValue == 0): drive.stop() else: if (uValue <= 3): self.direction = random.randint(0, 1) if (self.direction == 0): drive.turnLeft45() drive.pivotTurn45(30, 10) self.h.turnRight() time.sleep(1) else: drive.turnRight45() drive.pivotTurn45(10, 30) self.h.turnLeft() time.sleep(1) self.switcher = 1 else: drive.moveForward()
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 linefollowing(): BP.set_sensor_type(BP.PORT_3, BP.SENSOR_TYPE.EV3_COLOR_REFLECTED) sensor = 0 elapsedTime = 0 maxTime = 17 startTime = time.time() while (sensor < 90): try: sensor = BP.get_sensor(BP.PORT_3) # print(sensor) except brickpi3.SensorError as error: print(error) elapsedTime = round(time.time() - startTime, 1) if (elapsedTime <= maxTime): turning(sensor) else: drive.revPivotTurn45(-30, -15) drive.moveFast() time.sleep(0.1) drive.stop() break time.sleep(0.02)
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 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()
def right90(): """ Turn robot right 90 degrees by running left wheel """ dist = 21.0 drive.stop() rightWheel.reset() leftWheel.reset() move_state = True while move_state: log("target= {0} v actual= {1}".format(dist, leftWheel.dist)) if dist < leftWheel.dist: drive.stop() move_state = False else: drive.move(40, 0) drive.stop()
def left90(): """ Turn the robot 90 degrees left by running the right hand motor """ dist = 19.0 drive.stop() rightWheel.reset() leftWheel.reset() move_state = True while move_state: log("target= {0} v actual= {1}".format(dist, rightWheel.dist)) if dist < rightWheel.dist: drive.stop() move_state = False else: drive.move(0, 40) drive.stop() # Put encoder to known position align_encoder()
# drive from A to B drive.SetSpeed(speed, speed); print(encoder.get()) drive.sleep(1) # turn towards C drive.SetSpeed(-speed,speed); drive.sleep(0.7) #drive from B to C drive.SetSpeed(speed, speed); drive.sleep(1.5) # turn towards D drive.SetSpeed(speed,-speed); drive.sleep(0.7) # drive from C to D drive.SetSpeed(speed, speed); drive.sleep(1) # stop drive.stop(); while 1: sleep(10) drive.gpioCleanUP()
def destroy(): # Shutdown GPIO and Cleanup modules print('\n... Shutting Down...\n') drive.stop() # Make sure Bot is not moving when program exits drive.cleanup() # Shutdown all motor control GPIO.cleanup()
def avoidance(self): uValue = 70 uValue = self.getUltrasonic() print("Avoidance:" + str(uValue)) if (uValue <= 10): self.closeToObject = True if (uValue == 0): drive.stop() else: if (self.closeToObject == True): if (self.ScanValues[2] == 0): self.h.returnCentre() drive.stop() time.sleep(1) self.ScanValues[2] = self.getUltrasonic() print("Centre Scan Value:" + str(uValue)) if (self.i <= 9 and self.ScanValues[1] == 0): if (self.i == 0): scanV = self.h.scan(0) else: scanV = 3 if (scanV == 2): self.leftScanArray.append(self.getUltrasonic()) elif (scanV == 3): if (self.ScanValues[0] == 0): self.ScanValues[0] = self.getLowest( self.leftScanArray) print("Left Scan Value:" + str(self.ScanValues[0])) self.rightScanArray.insert(self.i, self.getUltrasonic()) self.i += 1 print("i = " + str(self.i)) # print("Right Scan Value:" + str(self.getUltrasonic())) elif (self.ScanValues[1] == 0): self.ScanValues[1] = self.getLowest(self.rightScanArray) print("Right Scan Value:" + str(self.ScanValues[1])) self.i = 0 elif (self.positionSet == False): if (self.ScanValues[1] > 60): self.h.turnLeft(0.7) self.direction = 1 self.headAngle = 1 elif (self.ScanValues[0] > 60): self.h.turnRight(0.7) self.direction = 0 self.headAngle = 2 else: self.h.returnCentre() self.direction = random.randint(0, 1) self.headAngle = 0 self.positionSet = True else: if (uValue < 5): if (self.headAngle == 0): if (self.direction == 0): drive.turnLeft90() self.h.turnRight(1) else: drive.turnRight90() self.h.turnLeft(1) elif (self.headAngle == 1): drive.turnRight45() self.h.returnCentre() self.h.turnLeft(1) else: drive.turnLeft45() self.h.returnCentre() self.h.turnRight(1) self.switcher = 1 else: drive.moveForward() else: drive.moveForward() self.h.scan(1)
def stop(self): drive.stop() self.manager.lbl_status.text = "Engine stopped"
def leftTurn(time=5): drive.drive("L", 50, 2, 0) drive.drive("R", 50, 2, 1) time.sleep(time) drive.stop()
decision, pixel_width = tracking.steering_decision(frame, LOWER, UPPER, STEERING_TOLERANCE, HEIGHT, WIDTH) if pixel_width != 0: distance_prediction = tracking.distance(pixel_width, REAL_WIDTH, FOCAL_LENGTH) else: distance_prediction = "No object" print(decision, distance_prediction) # Actions, based on previous decision 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:
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()
#--- stockpile ["stack", lambda: stackFile()], ["dump feeder material", lambda: dump_feeder.dump1()], ["flush feeder material", lambda: dump_feeder.dump2()], ["reclaim", lambda: reclaimer.reclaim("../data/" + lastStockpileName)], #--- feeder ["control feeder", "./shakekey.sh"], ["stop feeder", lambda: shaker.stop()], #--- drive ["flush stockpile", lambda: drive.flush()], ["drive to start pos", lambda: drive.moveToStockpile(0)], ["drive to end pos", lambda: drive.moveToStockpile(1)], ["drive to dump pos", lambda: drive.moveToMM(-400)], ["drive left", lambda: drive.run(-1000)], ["drive right", lambda: drive.run(1000)], ["drive stop", lambda: drive.stop()], #--- other ["colorcount", lambda: os.system("colorcount")], ["alarm", "./alarm.sh"], ["poweroff", lambda: os.system("poweroff")], ["reboot", lambda: os.system("reboot")], ["close", lambda: stop()] ] def stop(): global run run = False return 0 def stackFile(): global lastStockpileName
def on_press(key): #frame = vs.read() frame = 'Evangelion' if key == Key.esc: drive.stop() cv2.destroyAllWindows() #vs.stop() return False if key == Key.up: drive.move(Forward) print('foward') printtofile(frame, 'forward') if key == Key.down: drive.move(Reverse) print('reverse') printtofile(frame, 'reverse') if key == Key.left: drive.move(Left) print('left') printtofile(frame, 'left') if key == Key.right: drive.move(Right) print('right') printtofile(frame, 'right') if key == Key.page_up: drive.move(F_Right) print('f_right') printtofile(frame, 'f_right') if key == Key.home: drive.move(F_Left) print('f_left') printtofile(frame, 'f_left') if key == Key.page_down: drive.move(R_Right) print('r_right') printtofile(frame, 'r_right') if key == Key.end: drive.move(R_Left) print('r_left') printtofile(frame, 'r_left') if key == Key.f1: drive.move(Spin_F_Left) print('spot_f_left') printtofile(frame, 'spin_f_left') if key == Key.f2: drive.move(Spin_F_Right) print('spin_f_right') printtofile(frame, 'spin_f_left') if key == Key.f3: drive.move(Spin_R_Left) print('spin_r_left') printtofile(frame, 'spin_r_left') if key == Key.f4: drive.move(Spin_R_Right) print('spin_r_right') printtofile(frame, 'spin_r_right') if key == Key.f5: drive.move(Spot_F_Right) print('spot_f_right') printtofile(frame, 'r_left') if key == Key.f6: drive.move(Spot_F_Left) print('spot_f_left') printtofile(frame, 'spot_f_left') if key == Key.f7: drive.move(Spot_R_Right) print('spot_r_right') printtofile(frame, 'spo_r_left') if key == Key.f8: drive.move(Spot_R_Left) print('spot_r_left') printtofile(frame, 'r_left') #if key == Key.enter: # drive.stop() # print('stop') # printtofile(frame,'stop') if key == Key.delete: drive.stop() drive.cleanup() print('STOP AND CLEANUP')