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)
