UPDATED (PID added and WORKS :D)

 
from java.lang import String
from java.lang import Class
from java.awt import Rectangle
from org.myrobotlab.service import Runtime
from org.myrobotlab.service import OpenCV
from org.myrobotlab.opencv import OpenCVData
from com.googlecode.javacv.cpp.opencv_core import CvPoint;
from org.myrobotlab.service import OpenCV
 
xpid = Runtime.createAndStart("xpid","PID")
ypid = Runtime.createAndStart("ypid","PID")
xpid.setMode(1)
xpid.setOutputRange(-10, 10)
xpid.setPID(10.0, 5.0, 1.0)
xpid.setControllerDirection(0)
xpid.setSetpoint(160) # we want the target in the middle of the x
ypid.setMode(1)
ypid.setOutputRange(-10, 10)
ypid.setPID(10.0, 5.0, 1.0)
ypid.setControllerDirection(0)
ypid.setSetpoint(120)
# 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():
 
    #print 'found face at (x,y) ', msg_opencv_publishOpenCVData.data[0].x(), msg_opencv_publish.data[0].y()
    opencvData = msg_opencv_publishOpenCVData.data[0]
    global x
    global y
    global sposx
    global sposy
    global posx
    global posy
    global servox
    global servoy
    if (opencvData.getBoundingBoxArray().size() > 0) :
     rect = opencvData.getBoundingBoxArray().get(0)
     posx = rect.x
     posy = rect.y
     
     w = rect.width
     h = rect.height
     sposx = (w/2)
     sposy = (h/2)
     x = (posx + sposx)
     y = (posy + sposy)
     print x
     print y
     xpid.setInput(x)
     xpid.compute()
     servox = xpid.getOutput()
     print 'x servo' , servox
     ypid.setInput(y)
     ypid.compute()
     servoy = ypid.getOutput()
     print 'y servo' , servoy
     return object
 
# create a message route from opencv to python so we can see the coordinate locations
opencv.addListener("publishOpenCVData", python.name, "input", OpenCVData().getClass()); 
 
# set the input source to the first camera
opencv.capture()

 

GroG

11 years 6 months ago

Hi Alessandruino -

This line 

h = rect.hight 

needs to change to 

h = rect.height