#!/usr/bin/env python

import serial
import hal
import linuxcnc

# read value from Arduino serial port
PORT = "/dev/ttyACM1"
ser = serial.Serial(PORT,9600)

# create new hal component
photo = hal.component("photoresistance")
photo.newpin("out",hal.HAL_FLOAT,hal.HAL_OUT)
photo.ready()

# open "joint_angles.txt" for writing and create it if does not exist
outFileName= "joint_angles.txt"
file = open(outFileName,"w")

try:
 while 1:
  while ser.inWaiting():
    val = ord(ser.read())
    photo['out'] = val

    # if "val" is less than 8, save the current joint positions in the text file
    
    if val<=8:

       x = hal.get_value("hm2_7i80.0.encoder.00.position")
       y = hal.get_value("hm2_7i80.0.encoder.01.position")
       z = hal.get_value("hm2_7i80.0.encoder.02.position")
       a = hal.get_value("hm2_7i80.0.encoder.03.position")
       b = hal.get_value("hm2_7i80.0.encoder.04.position")
        
       file.write("X"+str(x)+" Y"+str(y)+" Z"+str(z)+" A"+str(a)+" B"+str(b)+'\n')
        

except KeyboardInterrupt:
 raise SystemExit