# A script to test tracking on the Raspberry Pi driving servos with the AdaFruit16ServoDriver service
# as at mrl development build version 2489
# a mashup of code taken from Mats:
# https://github.com/MyRobotLab/pyrobotlab/blob/master/home/Mats/Tracking.py
# and also from Grog:
# http://myrobotlab.org/content/tracking-results
#


#Define the x and y tracking servo pins 
#articulated neck servos

centreneckPin = 1 # vertical motion
mainneckPin = 2 # horizontal motion
xPin = 9; # horizontal motion
yPin = 10; # vertical motion

#set which camera to use. In my case, 0 references the Raspberry Pi camera
cameraIndex = 0

# set the port to which the Arduino is connected
arduinoPort = '/dev/ttyUSB0'

# start a tracker service instance
tracker = Runtime.start("tracker", "Tracking");

tracker.connect(arduinoPort, xPin, yPin, cameraIndex);

x = tracker.getX();
# invert if necessary
# x.setInverted(True);
x.setVelocity(20)
x.setMinMax(60,90)
#x.setInverted(True);
x.setRest(85)
x.rest()
 
y = tracker.getY();
y.setVelocity(20)
y.setInverted(True);
y.setMinMax(60,75)
y.setRest(70)
y.rest()



#start an Arduino service instance
#arduino = Runtime.start("tracker.controller","Arduino")

#define a tracker PID instance
pid = Runtime.start("tracker.pid","Pid")

#set the x and y PID values
#pid.setPID("x", 20.0, 5.0, 0.1);
#pid.setPID("y", 20.0, 5.0, 0.1);

opencv = Runtime.start("tracker.opencv","OpenCV")
pid.setPID("x", 10.0, 1.0, 0.1);
pid.setPID("y", 50.0, 1.0, 0.1);


#get the tracker opencv service instance
#opencv = Runtime.getService("tracker.opencv")
sleep(2);

#opencv.addFilter("PyramidDown1","PyramidDown")
#opencv.addFilter("Gray1","Gray")

#as at mrl development build 2423 this next piece is required on the Raspberry Pi (3) under javacv1.3 
#for opencv to return video frames
#frameGrabberType = "org.bytedeco.javacv.FFmpegFrameGrabber";
#opencv.captureFromResourceFile("/dev/video0");
#opencv.setFrameGrabberType(frameGrabberType);
#opencv.broadcastState();
#sleep(3);
#rest for a bit
#sleep(3);


#start the opencv video frame capture
opencv.capture();


#opencv.addFilter("lkOpticalTrack1","LKOpticalTrack")
#opencv.setDisplayFilter("lkOpticalTrack1")
#sleep(1)
#opencv.invokeFilterMethod("lkOpticalTrack1","samplePoint",160,120)

#start tracking

#1# tracker.startLKTracking()
#2# tracker.findFace()
#3# tracker.faceDetect()



# A script to test opencv tracking in MyRobotLab with an Arduino connected to a Raspberry Pi 3
# as at mrl development build version 2489
# a mashup of code taken from Mats:
# https://github.com/MyRobotLab/pyrobotlab/blob/master/home/Mats/Tracking.py
# and also from Grog:
# http://myrobotlab.org/content/tracking-results
#


#Define the x and y tracking servo pins 
#articulated neck servos

centreneckPin = 1 # vertical motion
mainneckPin = 2 # horizontal motion
xPin = 9; # horizontal motion
yPin = 10; # vertical motion

#set which camera to use. In my case, 0 references the Raspberry Pi camera
cameraIndex = 0

# set the port to which the Arduino is connected
arduinoPort = '/dev/ttyUSB0'

# start a tracker service instance
tracker = Runtime.start("tracker", "Tracking");

tracker.connect(arduinoPort, xPin, yPin, cameraIndex);

x = tracker.getX();
# invert if necessary
# x.setInverted(True);
x.setVelocity(20)
x.setMinMax(60,90)
#x.setInverted(True);
x.setRest(85)
x.rest()
 
y = tracker.getY();
y.setVelocity(20)
y.setInverted(True);
y.setMinMax(60,75)
y.setRest(70)
y.rest()



#start an Arduino service instance
#arduino = Runtime.start("tracker.controller","Arduino")

#define a tracker PID instance
pid = Runtime.start("tracker.pid","Pid")

#set the x and y PID values
#pid.setPID("x", 20.0, 5.0, 0.1);
#pid.setPID("y", 20.0, 5.0, 0.1);

opencv = Runtime.start("tracker.opencv","OpenCV")
pid.setPID("x", 10.0, 1.0, 0.1);
pid.setPID("y", 50.0, 1.0, 0.1);


#get the tracker opencv service instance
#opencv = Runtime.getService("tracker.opencv")
sleep(2);

#opencv.addFilter("PyramidDown1","PyramidDown")
#opencv.addFilter("Gray1","Gray")

#as at mrl development build 2423 this next piece is required on the Raspberry Pi (3) under javacv1.3 
#for opencv to return video frames
#frameGrabberType = "org.bytedeco.javacv.FFmpegFrameGrabber";
#opencv.captureFromResourceFile("/dev/video0");
#opencv.setFrameGrabberType(frameGrabberType);
#opencv.broadcastState();
#sleep(3);
#rest for a bit
#sleep(3);


#start the opencv video frame capture
opencv.capture();


#opencv.addFilter("lkOpticalTrack1","LKOpticalTrack")
#opencv.setDisplayFilter("lkOpticalTrack1")
#sleep(1)
#opencv.invokeFilterMethod("lkOpticalTrack1","samplePoint",160,120)

#start tracking

#1# tracker.startLKTracking()
#2# tracker.findFace()
#3# tracker.faceDetect()



Update tracking-arduino.py