#! /usr/bin/python

import time, random
from phydget import InterfaceKit, Servo, TextLCD

ik = InterfaceKit()
lcd = TextLCD()
ik.open(block=1)
#while not ik.getDeviceStatus():
#    time.sleep(0.05)
lcd.open(block=1)
time.sleep(0.5)

lcd.setBacklight(True)
print "attached", lcd.getDeviceStatus()
lcd.setDisplayString(0, "")
lcd.setDisplayString(1, "")

# on the little "ministick" sensor, the axis I'm using for up/down has a
# range of about 56 to 970, and the midpoint is about 518

count = 0

def poll():
    xin = ik.getSensorValue(5)
    x = xin / 1000.0
    x = 1.0 - x
    minp = -23; maxp = 231
    #xpos = (maxp-minp) * x + minp
    leftp = 36.436
    rightp = 202
    xpos = (rightp-leftp) * x + leftp
    lcd.setDisplayString(0, "Xin=%4d Xpos=%.1f" % (xin,xpos))

    yin = ik.getSensorValue(4)
    #y = yin / 1000.0
    yin = yin - 18
    y = (yin - 56) / (970.0 - 56)
    leftp = 37
    # center is 117
    rightp = 197
    ypos = (rightp-leftp) * y + leftp
    ypos = min(156, ypos)  # this is where the bracket gets in the way
    lcd.setDisplayString(1, "Yin=%4d Ypos=%.1f" % (yin,ypos))

    ik.setOutputState(0, random.randint(0,1))



    
while True:
    poll()
    time.sleep(0.1)