215 lines
6.4 KiB
Python
215 lines
6.4 KiB
Python
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)
|