# Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
# Use of this source code is governed by a BSD-style license that can be
# found in the LICENSE file.

""" This script tests the various functionalities of the Touchbot II

By running this script you can exercise all the features, moving the
robot, manipulating the fingers, etc.  This is useful for debugging and
development prior to more sophisticated uses of the robot being implemented.
"""

import time
from touchbotII import Touchbot


# Connect to the robot
bot = Touchbot()
bot.SetSpeed(Touchbot.SPEED_SLOW)

# Have the user specify two positions by manually adjusting the robot
pos1 = bot.CalibratePosition()
pos2 = bot.CalibratePosition()

# Move between the two positions measured earlier
bot.SetAngles(pos1)
bot.SetAngles(pos2)
bot.SetCartesian(pos1)
bot.SetCartesian(pos2)

# Extend, then retract each finger in turn
for f in Touchbot.ALL_FINGERS:
    bot.SetGPIO(f, 1)
    time.sleep(1)
    bot.SetGPIO(f, 0)

# Prime the nest, open and then close it
bot.SetGPIO(Touchbot.NEST_OPEN_VALVE, 1)
bot.SetGPIO(Touchbot.NEST_CLOSE_VALVE, 0)
bot.SetGPIO(Touchbot.NEST_OPEN_VALVE, 0)
bot.SetGPIO(Touchbot.NEST_CLOSE_VALVE, 1)
time.sleep(2)
bot.SetGPIO(Touchbot.NEST_OPEN_VALVE, 1)
bot.SetGPIO(Touchbot.NEST_CLOSE_VALVE, 0)

# Display several readings from the fingertip sensor
for i in range(10):
    bot.GetGPIO(Touchbot.FINGERTIP_DETECT)
    time.sleep(1)

# Test the robot's ability to change its fingertips
for size in range(len(Touchbot.NEST_POSITION_FILES)):
    bot.ManipulateFingertips([1, 1, 1, 1], size, True)
    fingers_present = bot.DetectFingertips()
    if not all(fingers_present):
        print "Some finger wasn't picked up!"
        break

    bot.ManipulateFingertips([1, 1, 1, 1], size, False)
    fingers_present = bot.DetectFingertips()
    if any(fingers_present):
        print "Some finger wasn't put back!"
        break