# cartesian_target.py
# Copyright, 2024, Garth Zeglin.
# Sample Webots controller file for moving a spherical vision target using a 2D cartesian mechanism.

print("cartesian_target.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))

# Fetch handles for the motors.
ymotor = robot.getDevice('ymotor')
zmotor = robot.getDevice('zmotor')

# Set time constants for the overall oscillation.
lateral_period = 30.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()

    # Move the target through a Lissajous pattern.
    yphase = t * (2*math.pi/lateral_period)
    ymotor.setPosition(1.0 * math.sin(yphase))
    zmotor.setPosition(1.0 + 0.25 * math.sin(2.25*yphase))