from java.lang import String
from java.lang import Class
from org.myrobotlab.service import Runtime
from org.myrobotlab.service import OpenCV
from com.googlecode.javacv.cpp.opencv_core import CvPoint;
from org.myrobotlab.service import OpenCV
 
# create or get a handle to an OpenCV service
opencv = Runtime.create("opencv","OpenCV")
opencv.startService()
# reduce the size - face tracking doesn't need much detail
# the smaller the faster
opencv.addFilter("PyramidDown1", "PyramidDown")
# add the face detect filter
opencv.addFilter("FaceDetect1", "FaceDetect")
 
def input():
  
  global sampleCount
  global xAvg 
  global yAvg
 
  x = msg_opencv_publish.data[0].x()
  panOffset = (320/2 - x) / pixelsPerDegree
  pan.moveTo(90 + xAvg/sampleCount)
  
    print 'found face at (x,y) ', msg_opencv_publish.data[0].x(), msg_opencv_publish.data[0].y()
    return object
 
# create a message route from opencv to python so we can see the coordinate locations
opencv.addListener("publish", python.name, "input", CvPoint().getClass()); 
 
# set the input source to the first camera
opencv.capture()
 
configType = 'michael'
 
# configuration variables - differ on each system
if configType == 'GroG':
  # GroG's config
  panServoPin = 9
  tiltServoPin = 10
  comPort = 'COM9'
  cameraIndex = 1
else:
  # michael's config
  panServoPin = 3
  tiltServoPin = 11
  comPort = 'COM3'
  cameraIndex = 1
 
 
# ///////////////////PID/////////////////////////////////////////////////////
# 2 PID services to adjust for X & Y of the open cv point and to map them
# to the pan and tilt servo values
# X : opencv 0 - 320 ->  pan 0 - 180
# Y : opencv 0 - 240 -> tilt 0 - 180
 
# xpid = Runtime.createAndStart("xpid","PID")
# ypid = Runtime.createAndStart("ypid","PID")
 
# xpid.setInputRange(0, 320)
# xpid.setOutputRange(0, 180)
# xpid.setPoint(160) # we want the target in the middle of the x
 
# ypid.setInputRange(0, 240)
# ypid.setOutputRange(0, 180)
# xpid.setPoint(160) # and the middle of the y
 
 
# ///////////////////ARDUINO/////////////////////////////////////////////////////
 
from time import sleep
from org.myrobotlab.service import Arduino
 
arduino = runtime.createAndStart('arduino','Arduino')
 
# set and open the serial device 
# arduino.setSerialDevice('/dev/ttyUSB0', 57600, 8, 1, 0)
arduino.setSerialDevice(comPort, 57600, 8, 1, 0)
 
sleep(3) # sleep because even after initialization the serial port still takes time to be ready
arduino.pinMode(16, Arduino.INPUT)
# arduino.digitalReadPollingStop(7)
arduino.analogReadPollingStart(16) # A2
sleep(1)
arduino.analogReadPollingStop(16)
 
# //////////////SERVOS////////////////////////////////////////////
from org.myrobotlab.service import Servo
 
pan = runtime.createAndStart('pan','Servo')
tilt = runtime.createAndStart('tilt','Servo')
 
# attach the pan servo to the Arduino on pin 2
# pan.attach(arduino.getName(),panServoPin) 
arduino.servoAttach(pan.getName(),panServoPin)
# attach the pan servo to the Arduino on pin 3
#tilt.attach(arduino.getName(),tiltServoPin) 
arduino.servoAttach(tilt.getName(),tiltServoPin)
 
pan = runtime.createAndStart('pan','Servo')
tilt = runtime.createAndStart('tilt','Servo')
 
# head shake 3 times and nod 3 times - checks arduino
# and servo connectivity
for pos in range(0,3):
  pan.moveTo(70)
  pan.moveTo(100)
  sleep(0.5)
 
pan.moveTo(90)
 
for pos in range(0,3):
  tilt.moveTo(70)
  tilt.moveTo(100)
  sleep(0.5)
 
tilt.moveTo(90)