from ti_hub import *
from servo import *
import brightns
from ti_system import *
import time

# Servo on OUT 3
out3 = servo("OUT 3")

def servo_setAngle(pin, angle):
  if (angle >= -90 and angle <= 90):
    pin.set_position(angle)
  else:
    raise ValueError("Servomotor angle have to be set between -90 and 90")

disp_clr()

a = 90
servo_setAngle(out3, a)
s = brightns.measurement()
s = s * 0.8
print('s=' + str(s))
v = s * 1.5
while v > s:
  v = brightns.measurement()
  print('L=' + str(v))
  time.sleep(0.5)
a = 0
servo_setAngle(out3, a)
time.sleep(3)
a = 90
servo_setAngle(out3, a)

while not escape():
  pass