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)