#!/usr/bin/env python3

# Copyright (c) 2018 Anki, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License in the file LICENSE.txt or at
#
#     https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""Drive Vector's wheels, lift and head motors directly

This is an example of how you can also have low-level control of Vector's motors
(wheels, lift and head) for fine-grained control and ease of controlling
multiple things at once.
"""

import time
import anki_vector


def main():
    args = anki_vector.util.parse_command_args()
    with anki_vector.Robot(args.serial) as robot:
        robot.behavior.drive_off_charger()

        # Tell the head motor to start lowering the head (at 5 radians per second)
        print("Lower Vector's head...")
        robot.motors.set_head_motor(-5.0)

        # Tell the lift motor to start lowering the lift (at 5 radians per second)
        print("Lower Vector's lift...")
        robot.motors.set_lift_motor(-5.0)

        # Tell Vector to drive the left wheel at 25 mmps (millimeters per second),
        # and the right wheel at 50 mmps (so Vector will drive Forwards while also
        # turning to the left
        print("Set Vector's wheel motors...")
        robot.motors.set_wheel_motors(25, 50)

        # wait for 3 seconds (the head, lift and wheels will move while we wait)
        time.sleep(3)

        # Tell the head motor to start raising the head (at 5 radians per second)
        print("Raise Vector's head...")
        robot.motors.set_head_motor(5)

        # Tell the lift motor to start raising the lift (at 5 radians per second)
        print("Raise Vector's lift...")
        robot.motors.set_lift_motor(5)

        # Tell Vector to drive the left wheel at 50 mmps (millimeters per second),
        # and the right wheel at -50 mmps (so Vector will turn in-place to the right)
        print("Set Vector's wheel motors...")
        robot.motors.set_wheel_motors(50, -50)

        # Wait for 3 seconds (the head, lift and wheels will move while we wait)
        time.sleep(3)

        # Stop the motors, which unlocks the tracks
        robot.motors.set_wheel_motors(0, 0)
        robot.motors.set_lift_motor(0)
        robot.motors.set_head_motor(0)


if __name__ == "__main__":
    main()
