"""A short SO-101 dance. Open this file in the app's Python editor.

Try changing beat or turns, then build and run the simulation again.
The robot module records a sequence; this script does not connect to motors.
"""
import robot

beat = 2.0       # Seconds per movement. Larger values make a slower dance.
turns = 2        # How many times to sway left and right.


def home():
    robot.move(shoulder_pan=0, shoulder_lift=0, elbow_flex=0,
               wrist_flex=0, wrist_roll=0, gripper=45,
               duration=beat, name="Ready")


home()
robot.wait(0.5)

# Sway and nod. Joint angles are absolute targets in degrees.
for repeat in range(turns):
    robot.move(shoulder_pan=12, shoulder_lift=-5, elbow_flex=8,
               wrist_flex=-5, wrist_roll=15,
               duration=beat, name="Sway right")
    robot.move(shoulder_pan=-12, shoulder_lift=3, elbow_flex=-5,
               wrist_flex=5, wrist_roll=-15,
               duration=beat, name="Sway left")

home()

# Give a small wave, then open and close the gripper partway.
for angle in [20, -10, 20, 0]:
    robot.move(wrist_roll=angle, duration=beat, name="Wave")
robot.grip(65, duration=beat)
robot.grip(45, duration=beat)

# Finish with a bow and return to the starting pose.
robot.move(shoulder_lift=6, elbow_flex=-8,
           duration=beat, name="Bow")
robot.wait(0.5)
home()
print("Dance ready. Try a different beat or number of turns.")