################################################################################
# Robotic Servo Arm
# 
# Created: 17-12.2015 by 0lab Team - www.0lab.it
# Authors: G. Gentile
#
# Materials:
# 1 x ST Nucleo shield
# 2 x 9g servo motor
# 1 x 3D printed interfacing (Thingiverse link: http://)
################################################################################

import streams
from servo import servo

s = streams.serial()


MyServoX=servo.Servo(D11.PWM,400,2400,500,3000)
MyServoY=servo.Servo(D10.PWM,400,2400,500,3000)

sleep(200)
print("All servo ON")
MyServoX.attach()
MyServoY.attach()
sleep(2000)
MyServoX.moveToDegree(0)
sleep(300)
MyServoX.moveToDegree(180)
sleep(300)
MyServoY.moveToDegree(0)
sleep(300)
MyServoY.moveToDegree(180)

while True:
    print("insert X:")
    line = s.readline()
    if line == 0:
        MyServoX.detach()
        print("Servo X OFF")
    MyServoX.moveToDegree (int(line))
    print("You wrote:", line)
    print("The servoX is", MyServoX.getCurrentDegree())
    print("insert Y:")
    line = s.readline()
    if line == 0:
        MyServoY.detach()
        print("Servo Y OFF")
    MyServoY.moveToDegree (int(line))
    print("You wrote:", line)
    print("The servoY is", MyServoX.getCurrentDegree())