First commit
This commit is contained in:
214
example_robots/keyboard_robot.py
Normal file
214
example_robots/keyboard_robot.py
Normal file
@@ -0,0 +1,214 @@
|
||||
import math
|
||||
|
||||
from controller import Keyboard
|
||||
from sr.robot3 import A0, A1, A2, OUT_H0, Colour, Robot
|
||||
|
||||
# Keyboard sampling period in milliseconds
|
||||
KEYBOARD_SAMPLING_PERIOD = 100
|
||||
NO_KEY_PRESSED = -1
|
||||
|
||||
CONTROLS = {
|
||||
"forward": (ord("W"), ord("I")),
|
||||
"reverse": (ord("S"), ord("K")),
|
||||
"left": (ord("A"), ord("J")),
|
||||
"right": (ord("D"), ord("L")),
|
||||
"sense": (ord("Q"), ord("U")),
|
||||
"see": (ord("E"), ord("O")),
|
||||
"led": (ord("Z"), ord("M")),
|
||||
"sucker_enable": (ord("X"), ord(",")),
|
||||
"sucker_disable": (ord("C"), ord(".")),
|
||||
"lift_up": (ord("R"), ord("P")),
|
||||
"lift_down": (ord("F"), ord(";")),
|
||||
"boost": (Keyboard.SHIFT, Keyboard.CONTROL),
|
||||
"angle_unit": (ord("B"), ord("B")),
|
||||
}
|
||||
|
||||
USE_DEGREES = False
|
||||
|
||||
|
||||
class KeyboardInterface:
|
||||
def __init__(self):
|
||||
self.keyboard = Keyboard()
|
||||
self.keyboard.enable(KEYBOARD_SAMPLING_PERIOD)
|
||||
self.pressed_keys = set()
|
||||
|
||||
def process_keys(self):
|
||||
new_keys = set()
|
||||
key = self.keyboard.getKey()
|
||||
|
||||
while key != NO_KEY_PRESSED:
|
||||
key_ascii = key & 0x7F # mask out modifier keys
|
||||
key_mod = key & (~0x7F)
|
||||
|
||||
new_keys.add(key_ascii)
|
||||
if key_mod:
|
||||
new_keys.add(key_mod)
|
||||
|
||||
key = self.keyboard.getKey()
|
||||
|
||||
key_summary = {
|
||||
"pressed": new_keys - self.pressed_keys,
|
||||
"held": new_keys,
|
||||
"released": self.pressed_keys - new_keys,
|
||||
}
|
||||
|
||||
self.pressed_keys = new_keys
|
||||
|
||||
return key_summary
|
||||
|
||||
|
||||
def angle_str(angle: float) -> str:
|
||||
if USE_DEGREES:
|
||||
degrees = math.degrees(angle)
|
||||
return f"{degrees:.1f}°"
|
||||
else:
|
||||
return f"{angle:.4f} rad"
|
||||
|
||||
|
||||
def print_sensors(robot: Robot) -> None:
|
||||
ultrasonic_sensor_names = {
|
||||
(2, 3): "Front",
|
||||
(4, 5): "Left",
|
||||
(6, 7): "Right",
|
||||
(8, 9): "Back",
|
||||
}
|
||||
reflectance_sensor_names = {
|
||||
A0: "Left",
|
||||
A1: "Center",
|
||||
A2: "Right",
|
||||
}
|
||||
touch_sensor_names = {
|
||||
10: "Front Left",
|
||||
11: "Front Right",
|
||||
12: "Rear Left",
|
||||
13: "Rear Right",
|
||||
}
|
||||
|
||||
print("Distance sensor readings:")
|
||||
for (trigger_pin, echo_pin), name in ultrasonic_sensor_names.items():
|
||||
dist = robot.arduino.ultrasound_measure(trigger_pin, echo_pin)
|
||||
print(f"({trigger_pin}, {echo_pin}) {name: <12}: {dist:.0f} mm")
|
||||
|
||||
print("Touch sensor readings:")
|
||||
for pin, name in touch_sensor_names.items():
|
||||
touching = robot.arduino.pins[pin].digital_read()
|
||||
print(f"{pin} {name: <6}: {touching}")
|
||||
|
||||
print("Reflectance sensor readings:")
|
||||
for Apin, name in reflectance_sensor_names.items():
|
||||
reflectance = robot.arduino.pins[Apin].analog_read()
|
||||
print(f"{Apin} {name: <12}: {reflectance:.2f} V")
|
||||
|
||||
|
||||
def print_camera_detection(robot: Robot) -> None:
|
||||
markers = robot.camera.see()
|
||||
if markers:
|
||||
print(f"Found {len(markers)} makers:")
|
||||
for marker in markers:
|
||||
print(f" #{marker.id}")
|
||||
print(
|
||||
f" Position: {marker.position.distance:.0f} mm, "
|
||||
f"{angle_str(marker.position.horizontal_angle)} right, "
|
||||
f"{angle_str(marker.position.vertical_angle)} up",
|
||||
)
|
||||
yaw, pitch, roll = marker.orientation
|
||||
print(
|
||||
f" Orientation: yaw: {angle_str(yaw)}, pitch: {angle_str(pitch)}, "
|
||||
f"roll: {angle_str(roll)}",
|
||||
)
|
||||
print()
|
||||
else:
|
||||
print("No markers")
|
||||
|
||||
print()
|
||||
|
||||
|
||||
robot = Robot()
|
||||
keyboard = KeyboardInterface()
|
||||
lift_height = robot.servo_board.servos[0].position
|
||||
|
||||
# Automatically set the zone controls based on the robot's zone
|
||||
# Alternatively, you can set this manually
|
||||
# ZONE_CONTROLS = 0
|
||||
ZONE_CONTROLS = robot.zone
|
||||
|
||||
assert ZONE_CONTROLS < len(CONTROLS["forward"]), \
|
||||
"No controls defined for this zone, alter the ZONE_CONTROLS variable to use in this zone."
|
||||
|
||||
print(
|
||||
"Note: you need to click on 3D viewport for keyboard events to be picked "
|
||||
"up by webots",
|
||||
)
|
||||
|
||||
while True:
|
||||
boost = False
|
||||
left_power = 0.0
|
||||
right_power = 0.0
|
||||
|
||||
keys = keyboard.process_keys()
|
||||
|
||||
# Actions that are run continuously while the key is held
|
||||
if CONTROLS["forward"][ZONE_CONTROLS] in keys["held"]:
|
||||
left_power += 0.5
|
||||
right_power += 0.5
|
||||
|
||||
if CONTROLS["reverse"][ZONE_CONTROLS] in keys["held"]:
|
||||
left_power += -0.5
|
||||
right_power += -0.5
|
||||
|
||||
if CONTROLS["left"][ZONE_CONTROLS] in keys["held"]:
|
||||
left_power -= 0.25
|
||||
right_power += 0.25
|
||||
|
||||
if CONTROLS["right"][ZONE_CONTROLS] in keys["held"]:
|
||||
left_power += 0.25
|
||||
right_power -= 0.25
|
||||
|
||||
if CONTROLS["boost"][ZONE_CONTROLS] in keys["held"]:
|
||||
boost = True
|
||||
|
||||
if CONTROLS["lift_up"][ZONE_CONTROLS] in keys["held"]:
|
||||
# constrain to [-1, 1]
|
||||
lift_height = max(min(lift_height + 0.05, 1), -1)
|
||||
robot.servo_board.servos[0].position = lift_height
|
||||
|
||||
if CONTROLS["lift_down"][ZONE_CONTROLS] in keys["held"]:
|
||||
# constrain to [-1, 1]
|
||||
lift_height = max(min(lift_height - 0.05, 1), -1)
|
||||
robot.servo_board.servos[0].position = lift_height
|
||||
|
||||
# Actions that are run once when the key is pressed
|
||||
if CONTROLS["sense"][ZONE_CONTROLS] in keys["pressed"]:
|
||||
print_sensors(robot)
|
||||
|
||||
if CONTROLS["see"][ZONE_CONTROLS] in keys["pressed"]:
|
||||
print_camera_detection(robot)
|
||||
|
||||
if CONTROLS["sucker_enable"][ZONE_CONTROLS] in keys["pressed"]:
|
||||
robot.power_board.outputs[OUT_H0].is_enabled = 1
|
||||
|
||||
if CONTROLS["sucker_disable"][ZONE_CONTROLS] in keys["pressed"]:
|
||||
robot.power_board.outputs[OUT_H0].is_enabled = 0
|
||||
|
||||
if CONTROLS["led"][ZONE_CONTROLS] in keys["pressed"]:
|
||||
robot.kch.leds[0].colour = Colour.MAGENTA
|
||||
robot.kch.leds[1].colour = Colour.MAGENTA
|
||||
robot.kch.leds[2].colour = Colour.MAGENTA
|
||||
elif CONTROLS["led"][ZONE_CONTROLS] in keys["released"]:
|
||||
robot.kch.leds[0].colour = Colour.OFF
|
||||
robot.kch.leds[1].colour = Colour.OFF
|
||||
robot.kch.leds[2].colour = Colour.OFF
|
||||
|
||||
if CONTROLS["angle_unit"][ZONE_CONTROLS] in keys["pressed"]:
|
||||
USE_DEGREES = not USE_DEGREES
|
||||
print(f"Angle unit set to {'degrees' if USE_DEGREES else 'radians'}")
|
||||
|
||||
if boost:
|
||||
# double power values but constrain to [-1, 1]
|
||||
left_power = max(min(left_power * 2, 1), -1)
|
||||
right_power = max(min(right_power * 2, 1), -1)
|
||||
|
||||
robot.motor_board.motors[0].power = left_power
|
||||
robot.motor_board.motors[1].power = right_power
|
||||
|
||||
robot.sleep(KEYBOARD_SAMPLING_PERIOD / 1000)
|
||||
Reference in New Issue
Block a user