# Robot walks forward
import time
from robot import NixRobot
import os


if __name__ == "__main__":
    STEPS = 20
    DURATION = 800
    FAST = 200
    program_begins = "audio/program_begins.wav"
    program_complete = "audio/program_complete.wav"
    robot = NixRobot("/dev/ttyUSB0")
    homing = (180.0, 23.0, 161.0, 110.0, 57.84, 220.0, 81.4, 129.0)
    robot.homing_position(homing)
    os.system("aplay " + program_begins)

    i = 0
    while True:
        robot.move_all_servos((178, 42, 145, 110, 58, 228, 70, 129))
        time.sleep(0.2)
        i += 1
        if i >= STEPS:
            break
        robot.move_all_servos((178, 21, 168, 110, 50, 216, 90, 129))
        time.sleep(0.2)
        i += 1
        if i >= STEPS:
            break

    robot.homing_position(homing)
    robot.disable_torque()
    os.system("aplay " + program_complete)