def _moveSteps(self, direction, steps: int, speed: int, is_blocking=False): """ Handles move commands from the directive. Right and left movement can under or over turn depending on the surface type. :param direction: the move direction :param steps: the duration in steps that translate into number of rotations :param speed: the speed percentage as an integer :param is_blocking: if set, motor run until duration expired before accepting another command """ print("Move command: ({}, {}, {}, {})".format(direction, speed, steps, is_blocking)) if direction in Direction.FORWARD.value: self.drive.on_for_rotations(SpeedPercent(speed), SpeedPercent(speed), steps, block=is_blocking) if direction in Direction.BACKWARD.value: self.drive.on_for_rotations(SpeedPercent(-speed), SpeedPercent(-speed), steps, block=is_blocking) if direction in Direction.LEFT.value: self._turn(direction, speed) self.drive.on_for_rotations(SpeedPercent(speed), SpeedPercent(speed), steps, block=is_blocking) offset = -1 self.index = self.new_index(self.index, offset) self.pointing = self.direction[self.index] if direction in Direction.RIGHT.value: self._turn(direction, speed) self.drive.on_for_rotations(SpeedPercent(speed), SpeedPercent(speed), steps, block=is_blocking) offset = 1 self.index = self.new_index(self.index, offset) self.pointing = self.direction[self.index] if direction in Direction.STOP.value: self.drive.off() self.patrol_mode = False self.enemy_not_detected = False print("STOP!! patrol mode = {} y enemy not detected = {}".format( self.patrol_mode, self.enemy_not_detected)) if direction in Direction.PAUSE.value: self.drive.off() print("Pause to kill the enemy")
def moveUntilDistanceAway(self, distance, speed): ''' the function makes the robot move until it is a certain distance away from an object distance is how far away the ultrasonic sensor is from an object ''' if self.tank is None: print("Tank Needed For All Uses Of moveUntilDistanceAway") sys.exit(1) if self.ultrasonicSensor != None: print("Distance:", distance) while self.ultrasonicSensor.distance_centimeters_continuous > distance: print("Distance:", self.ultrasonicSensor.distance_centimeters_continuous) self.tank.on(SpeedPercent(10), SpeedPercent(10)) self.tank.on(SpeedPercent(speed), SpeedPercent(speed)) self.tank.off() else: print("Error with ultrasonic sensor!")
def main(is_find_line=True): if is_find_line: find_line() # color_sensor.calibrate_white() while True: while (good_luminance() == True): drive.on(SpeedPercent(50), SpeedPercent(50)) rgb = color_sensor.rgb if (yellow_color(rgb) == True): print(str(rgb)) drive.stop(brake=True) force_back_and_turn_left() drive.stop(brake=True) if (green_color() == True): drive.stop(brake=True) sound.speak("Move aside, MEATBAGS!") find_track()
def turn_right(self): #0..-126 self.tower_position = TowerPosition.RIGHT self.leds.set_color("RIGHT", "RED") #self.scan_tower.on_for_degrees(SpeedPercent(20), 4, brake=True, block=False) self.scan_tower.on_for_degrees(SpeedPercent(6), 126, brake=True, block=True) self.leds.set_color("RIGHT", "BLACK")
def fifth_run(self): """Treehouse: Aim""" self.on_for_distance(SpeedPercent(30), 23) # Go forward 23 inches to the treehouse self.on_for_distance(SpeedPercent(15), 2) # Drop in the cubes moving forward slowly self.on_for_distance(SpeedPercent(-10), 2.5) # Slowly back away from tree self.on_for_distance( SpeedPercent(-100), 21.5) # Go backwards 21.5 inches forcing the attachment off self.move_tank.on_for_rotations(25, -25, .25) # Turn to the right self.on_for_distance(SpeedPercent(-75), 12) # Go backwards 12 inches into base sleep(3) self.on_for_distance(100, 11) # Go forward to circle self.on_for_distance(-100, 11) # Back up from circle self.move_tank.on_for_rotations(100, -100, .27) # Turn right self.on_for_distance(-100, 20) # Back up to wall
def turn_left(self): #0..126 self.tower_position = TowerPosition.LEFT self.leds.set_color("LEFT", "RED") #self.scan_tower.on_for_degrees(SpeedPercent(20), -4, brake=True, block=False) self.scan_tower.on_for_degrees(SpeedPercent(6), -126, brake=True, block=True) self.leds.set_color("LEFT", "BLACK")
def rotate_left_in_place(self, speed=50, amount=1.0, brake=True, block=True): self._wheels.on_for_rotations(100, SpeedPercent(speed), amount, brake=brake, block=block)
def deploy_color_sensor(): color_arm.on_for_rotations(SpeedPercent(5), 0.30) time.sleep(4.5) if color_sensor.color == 1: music.speak("I found something black on the surface of Mars") elif color_sensor.color == 2: music.speak("I found water on the surface of Mars") elif color_sensor.color == 3: music.speak("I found a plant on the surface of Mars") elif color_sensor.color == 4: music.speak("I found something yellow on the surface of Mars") elif color_sensor.color == 5: music.speak("I found a rock on the surface of Mars") elif color_sensor.color == 6: music.speak("I found something white on the surface of Mars") elif color_sensor.color == 7: music.speak("I found a rock on the surface of Mars") color_arm.on_for_rotations(SpeedPercent(-15), 0.3) time.sleep(3)
def find_line(): drive.on_for_seconds(100, 100, 4) left_motor = SpeedPercent(50) right_motor = SpeedPercent(25) turn_amount = 10 while (good_luminance() == False): timer_countdown = 0 drive.on_for_seconds(left_motor, right_motor, 1, block=False) while (good_luminance() == False and timer_countdown < 0.1 * turn_amount): timer_countdown += 0.01 sleep(0.01) # do nothing drive.stop() right_motor, left_motor = left_motor, right_motor drive.stop(brake=True)
def calibrate_compass(): tank_drive.on(0, 0) time.sleep(0.1) compass.command = 'BEGIN-CAL' steer_drive.on(100, SpeedPercent(75)) while (gyro.value() < 360): time.sleep(0.1) tank_drive.on(0, 0) compass.command = 'END-CAL' return "orient_toward_goal"
def backtrack(direction): turn_amount = 1 if (direction == "left"): left_motor = SpeedPercent(30) right_motor = SpeedPercent(-30) elif (direction == "right"): left_motor = SpeedPercent(-30) right_motor = SpeedPercent(30) while (not good_luminance()): drive.on_for_seconds(left_motor, right_motor, 0.1 * turn_amount, block=False) timer_countdown = 0 while (not good_luminance() and timer_countdown < 0.1 * turn_amount): timer_countdown += 0.01 sleep(0.01) # do nothing drive.stop() if (good_luminance()): #intended overshoot to get to the center of the tape #should be modified to rely on the intensity of light with a max value similar to the one in this if clause drive.on_for_seconds(left_motor, right_motor, 0.2) break else: right_motor, left_motor = left_motor, right_motor turn_amount *= 2 if (direction == "left"): direction = "right" else: direction = "left" last_backtrack_direction = direction
def ros_drive_callback(self, data): try: print('x: {} z: {}'.format(data.linear.x, data.angular.z)) self.callback_exit = False x = data.linear.x z = data.angular.z default_speed = 20 speed_factor = 100 turn_factor = 0.628 if self.ts.is_pressed: self.random_turn() else: if x == 0 and z != 0: if z > 0: print('left') mdiff.turn_left(SpeedPercent(default_speed), degrees(abs(z)), brake=False, block=False) elif z < 0: print('right') mdiff.turn_right(SpeedPercent(default_speed), degrees(abs(z)), brake=False, block=False) elif x > 0: print('forward') steering_drive.on( degrees(z) * turn_factor, SpeedPercent(x * speed_factor)) elif x < 0: print('backward') steering_drive.on( degrees(z) * turn_factor, SpeedPercent(x * speed_factor)) elif x == 0 and z == 0: print('stop') steering_drive.on(0, SpeedPercent(0)) self.callback_exit = True except Exception as e: print(e)
def start(self): # go until wall self.goUntilDistanceFromWall(41) # turn right self.moveTank.on_for_rotations(SpeedPercent(-40), SpeedPercent(40), 1.33) #self.steeringDrive.on(100, SpeedPercent(40)) # go until wall self.goUntilDistanceFromWall(36.4) # turn right self.moveTank.on_for_rotations(SpeedPercent(-40), SpeedPercent(40), 1.33) #self.steeringDrive.on(100, SpeedPercent(40)) sleep(4) self.steeringDrive.on_for_seconds(0, SpeedPercent(-100), 6)
def _move(self, direction, is_blocking=False): """ Handles move commands from the directive. Right and left movement can under or over turn depending on the surface type. :param direction: the move direction :param duration: the duration in seconds :param speed: the speed percentage as an integer :param is_blocking: if set, motor run until duration expired before accepting another command """ print("Move command: ({}, {}, {}, {})".format(direction, speed, is_blocking), file=sys.stderr) if direction in Direction.OPEN.value: self.drive.on_for_seconds(SpeedPercent(200), SpeedPercent(200), 5, block=is_blocking) if speed in Speed.SPEED.value: print("test!")
def test_tone(self): flip = 1 s = Sound() tank_drive = MoveTank(OUTPUT_A, OUTPUT_D) for x in range(4): flip *= -1 tank_drive.on_for_seconds(SpeedPercent(30 * flip), SpeedPercent(30 * flip), 1, True, True) s.tone([(392, 350, 100), (492, 350), (292, ), ()]) sleep(3) for x in range(4): flip *= -1 tank_drive.on_for_seconds(SpeedPercent(30 * flip), SpeedPercent(30 * flip), 1, True, True) s.tone([(392, 350, 100), (492, 350), (292, ), ()], play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE) sleep(3)
def skip_test_play_file(self): flip = 1 s = Sound() tank_drive = MoveTank(OUTPUT_A, OUTPUT_D) for x in range(2): flip *= -1 tank_drive.on_for_seconds(SpeedPercent(30 * flip), SpeedPercent(30 * flip), 1, True, True) s.play_file('inputFiles/bark.wav') sleep(3) for x in range(4): flip *= -1 tank_drive.on_for_seconds(SpeedPercent(30 * flip), SpeedPercent(30 * flip), 1, True, True) s.play_file('inputFiles/bark.wav', play_type=Sound.PLAY_NO_WAIT_FOR_COMPLETE) sleep(3)
def main(): program_running = 0 MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("outD") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") MA.on_for_degrees(SpeedPercent(50), 9000)
def get_next_turn(self): deg_p = 0 speed_left = 0 speed_rigth = 0 # Next direction dir = turns[self.cur_turn_i] if dir == "left": deg_p = -TURN_DEG speed_left = SpeedPercent(-10) speed_rigth = SpeedPercent(10) elif dir == "right": deg_p = TURN_DEG speed_left = SpeedPercent(10) speed_rigth = SpeedPercent(-10) self.cur_turn_i = self.cur_turn_i + 1 return [speed_left, speed_rigth, deg_p]
def _moveToBase(self, playerIndex: int, playerCount: int): print("Reset to base", file=sys.stderr) time.sleep(.50) angle = 180 / playerCount turnAngle = angle if playerIndex > 1: turnAngle = angle * (playerIndex - 1) if playerIndex == 1: turnAngle = 0 print("Angle: ({})".format(turnAngle), file=sys.stderr) self.turnMotor.on_for_degrees(SpeedPercent(15), turnAngle, True)
def callback_move(ch, method, properties, body): tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) if (method.routing_key.find('.turn') != -1): logging.info('Turn message processed') degree = int.from_bytes(body[0], byteorder='big') torque = int.from_bytes(body[4], byteorder='big') tank_drive.on_for_seconds(SpeedPercent(-torque), SpeedPercent(torque), 1) return if (method.routing_key.find('.distance') != -1): logging.info('Move message processed') distance = int.from_bytes(body[0], byteorder='big') torque = int.from_bytes(body[4], byteorder='big') tank_drive.on_for_seconds(SpeedPercent(torque * move_direction), SpeedPercent(torque * move_direction), 1) return
def on_custom_mindstorms_gadget_control(self, directive): """ Handles the Custom.Mindstorms.Gadget control directive. :param directive: the custom directive with the matching namespace and name """ try: payload = json.loads(directive.payload.decode("utf-8")) print("Control payload: {}".format(payload)) control_type = payload["type"] if control_type == "turn": # Scrunch the page and turn the page. self.page_starter.on_for_rotations(SpeedPercent(20), -0.47) self.page_turner.on_for_rotations(SpeedPercent(30), 1) if control_type == "read": # Set variable so the read thread will send messages. self._read_cmd = True except KeyError: print("Missing expected parameters: {}".format(directive))
def _move(self, direction, duration, speed, is_blocking=False): """ Handles move commands from the directive. Right and left movement can under or over turn depending on the surface type. :param direction: the move direction :param duration: the duration in seconds :param speed: the speed percentage as an integer :param is_blocking: if set, motor run until duration expired before accepting another command """ print("Move command: ({}, {}, {}, {})".format(direction, speed, duration, is_blocking)) if direction in Direction.FORWARD.value: self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking) # self.drive.run_timed(speed_sp=speed, time_sp=duration) # self.drive.run_timed(speed_sp=750, time_sp=2500) if direction in Direction.BACKWARD.value: self.drive.on_for_seconds(SpeedPercent(-speed), SpeedPercent(-speed), duration, block=is_blocking) # self.drive.run_timed(speed_sp=-speed, time_sp=duration) if direction in Direction.STOP.value: self.drive.off() self.patrol_mode = False
def follow_line(self): try: # Follow the line for 4500ms self.tank.follow_line( kp=11, ki=0.05, kd=3.2, speed=SpeedPercent(30), follow_for=follow_indef, prox_treshold=30, ) except Exception: self.tank.stop() raise
def rotate_degrees(self, degrees, reverse_before_continue=True, speed_percentage=35): """ Rotates the Robot. :param degrees: Number of degrees the Robot is moved from its current position. :param reverse_before_continue: True if Robot needs to reverse before turning, False if not. :param speed_percentage: Speed at which the Robot turns. Percentage between 0 and 100. """ self.tank_drive.stop() if reverse_before_continue: self.reverse_for_rotations(1) self.tank_drive.turn_left(SpeedPercent(speed_percentage), degrees) self.start_drive()
def _startRide(self, lapCount): lapsLeft = lapCount while lapsLeft > 0: #Cars passing light sensor at base of chain lift hill #signal beginning of lap if self.color.reflected_light_intensity > 10: #SpeedPercent(100), 35 required to get cars up chain lift hill #and around track bend at top self.chainLift.on_for_rotations(SpeedPercent(100), 35) lapsLeft = lapsLeft - 1 time.sleep(2) time.sleep(.5)
def test_units(self): clean_arena() populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')]) m = Motor() self.assertEqual(SpeedPercent(35).get_speed_pct(m), 35) self.assertEqual(SpeedDPS(300).get_speed_pct(m), 300 / 1050 * 100) self.assertEqual(SpeedNativeUnits(300).get_speed_pct(m), 300 / 1050 * 100) self.assertEqual(SpeedDPM(30000).get_speed_pct(m), (30000 / 60) / 1050 * 100) self.assertEqual(SpeedRPS(2).get_speed_pct(m), 360 * 2 / 1050 * 100) self.assertEqual(SpeedRPM(100).get_speed_pct(m), (360 * 100 / 60) / 1050 * 100)
def _moveToPlayer(self, playerIndex: int, playerCount: int, oneTimeMove: bool): angle = -180 / playerCount turnAngle = angle print("Moving to player: ({}) out of ({})".format(playerIndex, playerCount), file=sys.stderr) if oneTimeMove == True: if playerIndex > 1: turnAngle = angle * (playerIndex - 1) if playerIndex == 1: turnAngle = 0 print("Angle: ({})".format(turnAngle), file=sys.stderr) self.turnMotor.on_for_degrees(SpeedPercent(15), turnAngle, True) time.sleep(.25)
def move(self, speed=25): """ Move forward as a tank until it hits a black square and update the position :param speed: The speed to move :return: None """ self.position[0] += self.direction[0] self.position[1] += self.direction[1] #We start on a black square so we initalise to being on a white square self.black_square_sensor.start_reading(count=5, init_val=100, interval=0.1, wait_time=1) #Until we hit a black square, just keep moving forward self.tank.on(SpeedPercent(speed),SpeedPercent(speed)) #Block until we are over a black square while self.black_square_sensor.above_threshold(): continue self.tank.off() self.report_black_square() robot.correction()
def _move(self, direction, duration: int, speed: int, is_blocking=False): """ Handles move commands from the directive. Right and left movement can under or over turn depending on the surface type. :param direction: the move direction :param duration: the duration in seconds :param speed: the speed percentage as an integer :param is_blocking: if set, motor run until duration expired before accepting another command """ print("Move command: ({}, {}, {}, {})".format(direction, speed, duration, is_blocking), file=sys.stderr) if direction in Direction.FORWARD.value: self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking) if direction in Direction.BACKWARD.value: self.drive.on_for_seconds(SpeedPercent(-speed), SpeedPercent(-speed), duration, block=is_blocking) if direction in (Direction.RIGHT.value + Direction.LEFT.value): self._turn(direction, speed) self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking) if direction in Direction.STOP.value: self.drive.off() self.patrol_mode = False
def _activate(self, command, speed=50): """ Handles preset commands. :param command: the preset command :param speed: the speed if applicable """ print("Activate command: ({}, {})".format(command, speed), file=sys.stderr) if command in Command.MOVE_CIRCLE.value: self.drive.on_for_seconds(SpeedPercent(int(speed)), SpeedPercent(5), 12) if command in Command.MOVE_SQUARE.value: for i in range(4): self._move("right", 2, speed, is_blocking=True) if command in Command.PATROL.value: # Set patrol mode to resume patrol thread processing self.patrol_mode = True if command in Command.SENTRY.value: self.sentry_mode = True self._send_event(EventName.SPEECH, {'speechOut': "Sentry mode activated"}) # Perform Shuffle posture self.drive.on_for_seconds(SpeedPercent(80), SpeedPercent(-80), 0.2) time.sleep(0.3) self.drive.on_for_seconds(SpeedPercent(-40), SpeedPercent(40), 0.2) self.leds.set_color("LEFT", "YELLOW", 1) self.leds.set_color("RIGHT", "YELLOW", 1)