# spirals.py
#
# Sample Webots controller file for driving the
# underactuated spirals device.
#
# No copyright, 2020-2022, Garth Zeglin.  This file is
# explicitly placed in the public domain.

print("loading spirals.py...")

# Import the Webots simulator API.
from controller import Robot

# Import the standard Python math library.
import math

print("spirals.py waking up...")

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

# Request a proxy object representing the robot to
# control.
robot = Robot()

# Fetch handle for the 'base' joint motor.
motor1 = robot.getDevice('j1')
motor2 = robot.getDevice('j2')

# Run an event loop until the simulation quits,
# indicated by the step function returning -1.
while robot.step(EVENT_LOOP_DT) != -1:

    # Read simulator clock time.
    t = robot.getTime()

    # Compute and apply new base joint actuator torque.
    # In this example, the excitation is only based on
    # time, but could also be a function of the joint
    # positions.
    theta = 10 * math.sin(0.1*t)
    motor1.setPosition(theta)
    motor2.setPosition(-2*theta)