# 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
#

xPin = 0;
yPin = 1;

arduinoPort = "/dev/ttyAMA0";

cameraIndex = 0;


#start an AdaFruit16C I2C servo driver instance
adaFruit16c3 = Runtime.createAndStart("AdaFruit16C3","Adafruit16CServoDriver")

#start a Raspberry Pi instance
raspi = Runtime.createAndStart("RasPi","RasPi")

#attach the AdaFruit16C I2C servo driver to the Raspberry Pi
adaFruit16c3.setController("RasPi","1","0x42")

#set the frequency for the AdaFruit16C I2C servo driver to 50 Hz
adaFruit16c3.setPWMFreq(0,50) 



#start and connect a Virtual Arduino instance
virtual = Runtime.start("virtual", "VirtualArduino");
virtual.connect(arduinoPort);


#start a tracker instance
tracker = Runtime.start("tracker", "Tracking");
x = tracker.getX();
# invert if necessary
# x.setInverted(True);

y = tracker.getY();
# invert if necessary
# y.setInverted(True);


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

#attach x and y servos to AdaFruit servo driver
tracker.x.attach(adaFruit16c3,xPin,85,20);
tracker.y.attach(adaFruit16c3,yPin,70,20);

tracker.x.setVelocity(20);
tracker.x.setMinMax(60,90);
#x.setInverted(True);
tracker.x.setRest(85);
tracker.x.rest();
tracker.y.setVelocity(20);
tracker.y.setInverted(True);
tracker.y.setMinMax(60,75);
tracker.y.setRest(70);
tracker.y.rest();

#adjust PID values to suit
tracker.pid.setPID("tracker.x", 5.0, 1.0, 0.1);
tracker.pid.setPID("tracker.y", 5.0, 1.0, 0.1);

opencv = tracker.getOpenCV();
#opencv.broadcastState();

# do lk optical point tracking
# tracker.startLKTracking();

# do face tracking
#tracker.faceDetect();

@@ -325,16 +325,29 @@
 ults%0A#%0A%0A
+import time%0A%0A
 xPin = 0
@@ -1220,16 +1220,74 @@
 ndex);%0A%0A
+#small delay here to resolve servo/attach glitch%0Asleep(5)%0A
 #attach 
@@ -1361,18 +1361,18 @@
 c3,xPin,
-85
+70
 ,20);%0Atr
@@ -1400,25 +1400,25 @@
 it16c3,yPin,
-7
+6
 0,20);%0A%0Atrac
@@ -1512,10 +1512,10 @@
 est(
-85
+70
 );%0At
@@ -1607,17 +1607,17 @@
 tMinMax(
-6
+5
 0,75);%0At
@@ -1625,33 +1625,33 @@
 acker.y.setRest(
-7
+6
 0);%0Atracker.y.re
@@ -1684,16 +1684,60 @@
 to suit%0A
+pid = Runtime.start(%22tracker.pid%22,%22Pid%22)%0A%0A%0A#
 tracker.
@@ -1776,16 +1776,17 @@
 , 0.1);%0A
+#
 tracker.
@@ -1809,17 +1809,86 @@
 ker.y%22, 
-5
+20.0, 1.0, 0.1);%0A#pid.setPID(%22x%22, 10.0, 1.0, 0.1);%0A#pid.setPID(%22y%22, 50
 .0, 1.0,
@@ -1895,16 +1895,17 @@
  0.1);%0A%0A
+#
 opencv =
@@ -1949,24 +1949,170 @@
 stState();%0A%0A
+opencv = Runtime.start(%22tracker.opencv%22,%22OpenCV%22)%0Apid.setPID(%22x%22, 10.0, 1.0, 0.1);%0Apid.setPID(%22y%22, 20.0, 1.0, 0.1);%0Asleep(2);%0Aopencv.capture();%0A%0A%0A
 # do lk opti

Update tracking-i2c.py