# boom_wheel
# Copyright, 2024, Garth Zeglin.
# Sample Webots controller file for driving a wheel mounted on a passive boom.

print("boom_wheel.py waking up.")

# Import standard Python libraries.
import math

# Import the Webots simulator API.
from controller import Robot

# Define the time step in milliseconds between controller updates.
EVENT_LOOP_DT = 20

# Request a proxy object representing the robot to control.
robot = Robot()
robot_name = robot.getName()
print("%s: controller connected." % (robot_name))

# Connect to the radio receiver.
receiver = robot.getDevice('receiver')
receiver.enable(100) # milliseconds of sampling period
message_base_address = "/" + robot_name

# Fetch handles for the motor.
motor = robot.getDevice('motor')

# Enable velocity control mode.
motor.setPosition(math.inf)
speed = -5.0

# run loop to execute a periodic script until the simulation quits.
# If the controller returns -1, the simulator is quitting.
while robot.step(EVENT_LOOP_DT) != -1:
    # Read simulator clock time.
    t = robot.getTime()

    # Set the velocity controller target.
    motor.setVelocity(speed)

    # Process any radio messages.
    while receiver.getQueueLength() > 0:
        packet = receiver.getString()
        tokens = packet.split()
        if len(tokens) >= 2:
            if tokens[0].startswith(message_base_address):
                if tokens[0] == message_base_address + '/speed':
                    speed = float(tokens[1])
                    print("%s: set speed to %f" % (robot_name, speed))
        # done with packet processing, advance to the next packet
        receiver.nextPacket()