Huskylens with servo advice

If you look at the x and y values of the static object you will see that while y is absolutely stable the x values are not ...

The range is from 146 to 166 (-14 to +6 from a center position of 160).

However I am curious how the data change over a longer time.

It would make the evaluation easier for me if you could use the code from post 14 and copy the results in code tags (easier to copy the whole text for evaluation in a spreadsheet, the code window will alos have some scrollbars to avoid long posts).

In the meantime I downloaded the HuskyLens lib from github so that I can at least compile (but not test) ....

1 Like

Your help is allowing me to learn much more about arduino - thanks Tony

1	83	161
10024	147
41	10279	146
42	10534	147
43	10789	147
5	146
35	8750	147
36	9005	147
37	9260	146
38	9515	147
39	9769	146
40	10024	147
41	10279	146
42	10534	147
43	10789	147
5	146
35	8750	147
36	9005	147
37	9260	146
38	9515	147
39	9769	146
40	10024	147
41	10279	146
42	10534	147
43	10789	147
5	146
35	8750	147
36	9005	147
37	9260	146
38	9515	147
39	9769	146
40	10024	147
41	10279	146
42	10534	147
43	10789	147
1	69	147
2	324	147
3	578	147
4	833	147
5	1088	146
6	1343	147
7	1598	147
8	1853	147
9	2108	147
10	2363	147
11	2618	147
12	2873	147
13	3128	147
14	3383	147
15	3637	147
16	3892	147
17	4147	147
18	4402	147
19	4657	147
20	4912	146
21	5167	147
22	5422	146
23	5677	147
24	5932	146
25	6187	147
26	6441	147
27	6696	147
28	6951	147
29	7206	147
30	7461	147
31	7716	146
32	7971	147
33	8225	146
34	8480	147
35	8735	146
36	8990	147
37	9245	147
38	9500	147
39	9755	147
40	10010	147
41	10265	146
42	10520	147
43	10775	147
44	11030	147
45	11285	147
46	11540	146
47	11795	147
48	12050	147
49	12305	147
50	12560	147
51	12815	147
52	13070	147
53	13325	146
54	13580	146
55	13835	146
56	14090	147
57	14345	147
58	14600	147
59	14855	147
60	15110	146
61	15365	147
62	15620	147
63	15875	147
64	16130	146
65	16385	147

Here is my (hopefully correct) analysis of the Pan/Tilt sketch:

/*
To make the sketch more readable/understandable:

	"volatile" was removed as it is not required
	mind_n_currentx was changed to currenty 
	mind_n_currenty was changed to currenty
	xC was introduced for the recent xCenter value 
	yC was introduced for the recent yCenter value 
	abs() was removed as constrain(..,0,120) does not return any negative values 
	servo_9  changed to servo_horizontal as this servo moves along the x axis
	servo_10 changed to servo_vertical as this servo moves along the y axis
	
*/


Servo servo_horizontal;
Servo servo_vertical;

float currentx = 40;
float currenty = 150;
int xC;
int yC;

void setup() {
	THIS TEXT IS NOT WRITTEN TO COMPILE 
	JUST TO EXPLAIN THE PAN/TILT SKETCH!!!
}


void loop(){
	huskylens.request();

	xC = huskylens.readBlockParameter(1).xCenter;
	yC = huskylens.readBlockParameter(1).yCenter;
	
	if ((xC > 190)) {
		currentx -= 1.5;
		servo_horizontal.angle(constrain(currentx, 0, 120));
	}

	else if (xC >10) && xC < 130) {
		currentx += 1.5;
		servo_horizontal.angle(constrain(currentx, 0, 120));
	}

	if (yC > 150) {
		currenty += 1.0;
		servo_vertical.angle(constrain(currenty, 0, 120));
	}
	else if (yC > 10) && (yC < 90) {
		currenty -= 1;
		servo_vertical.angle(constrain(currenty, 0, 120));
	}
}
/*

Explanation:
	
The sketch starts with 
  currentx = 40
  currenty = 150
and changes currentx and currenty in accordance with the following rules

If xCenter > 190 					then currentx -= 1.5
If xCenter > 10 && xCenter < 130 	then currentx += 1.5

If yCenter > 150					then currenty += 1.0
If yCenter > 10 && yCenter <  90 	then currenty -= 1.0

If one of the two xCenter-ifs is met servo_horizontal is moved to (constrain(currentx,0,120)) which is an angle between 0 and 120 degrees.
If one of the two yCenter-ifs is met servo_vertical   is moved to (constrain(currenty,0,120)) which is an angle between 0 and 120 degrees.

Evaluation:

currentx will be reduced   while xCenter is larger than 190 pixels
currentx will be increased while xCenter is larger than 10 and less than 130 pixels
currentx will not be changed if xCenter is equal or less than 10
currentx will not be changed if xCenter is equal or between 130 and 190 pixels


currenty will be increased while yCenter is larger than 150 pixels
currenty will be reduced   while yCenter is larger than 10 and less than 90 pixels
currenty will not be changed if yCenter is equal or less than 10
currenty will not be changed if yCenter is equal or between 90 and 150 pixels


*/

This text is __not__ment to compile, just a simplified text to analyse what the sketch is doing.

In a nutshell for the horizontal servo:

  • The horizontal servo does not move while Huskylens produces xCenter values equal or between 130 and 190 pixels.
  • From 10 to 129 it moves the servo step by step to a larger angle
  • Above 190 it moves the servo step by step to a lower angle.
1 Like

Wow, that is amazing! Do you see that the xCenter is just moving by one pixel? That is much more stable than you ever had before in this thread.

If this is a repetitive feature we can trust the Huskylens algorithm :wink:

We could try to use the basic principle of the Pan/Tilt sketch and add it to your start code, alright?

1 Like

Thats brilliant one pixel if great! Happy to go with anything that you can come up with!

I have tried using the DFRobot_Servo.h but just cannot get it working, I think its my lack of understanding of the arduino compiler?

This is a quick shot ... It compiles at least :wink:

I have tried to integrate the Pan algorithm to your first sketch, removed all the averaging etc.

Could you test it with the stable object first?

And then slightly move the object to see how it reacts.

There should be no movement/correction while the xCenter is between 130 and 190 ... but movements below 130 (unless less than or equal to 10) and above 190.

/***********************************************
*  HUSKYLENS face recog PAN (servo)  *
*  Author:                                                  *
*  Date:06/10/24 - working                        *
*  Camera protocol = I2C                          *
*  Target Micro - Uno                                 *
 ***********************************************/
/*

 2024/01/13
  
 Compiled but not tested ...
 ec2021

*/

#include <HUSKYLENS.h>
#include <Wire.h>
#include <Servo.h>

HUSKYLENS huskylens;
Servo servo;

int servoPosition = 90;  // Initial position
float xPos = 90.0;

void setup() {
  servo.attach(9);  // Connect the servo to pin 9
  servo.write(servoPosition);
  Serial.begin(115200);
  Wire.begin();
  while (!huskylens.begin(Wire)) {
    Serial.println(F("HUSKYLENS not connected!"));
    delay(100);
  }
  huskylens.writeAlgorithm(ALGORITHM_FACE_RECOGNITION);
}

void loop() {
  if (!huskylens.request()) return;

  for (int i = 0; i < huskylens.countBlocks(); i++) {
    HUSKYLENSResult result = huskylens.getBlock(i);
    int x_center = result.xCenter;
    int width = result.width;
    int y_center = result.yCenter;
    int height = result.height;
    int ID = result.ID;
    Serial.print("Object tracked at X: ");
    Serial.print(x_center);
    Serial.print(", Y: ");
    Serial.print(y_center);
    Serial.print(", Width: ");
    Serial.print(width);
    Serial.print(", Height: ");
    Serial.println(height);
    Serial.print("Tracked ID: ");
    Serial.println(ID);
    byte mode = 0;
    if (x_center > 10 && x_center < 130) { mode = 1; }
    if (x_center > 190) { mode = 2; }
    switch(mode){
      case 0:  // No change with x_center below 10 or between 130 and 190
         break;
      case 1:   // Increase servo angle
           moveServo(1.5);
        break;
      case 2:   // Reduce servo angle
           moveServo(-1.5);
        break;
    }
    delay(250);  // Add a delay to allow the servo to move to the new position
  }
}

void moveServo(int delta){
  xPos += delta;
  servoPosition = constrain(xPos,0,120);
  servo.write(servoPosition);
}

I am not really happy with the algorithm because the float variable (as used in the original) can change its value without any limitations. It's a try but in case it works would have to be improved!

1 Like

That works! Thank you so much!!! it is now very stable, you are a genius!

Not a genius, just someone with little experience and knowledge ... :wink:

What happens if you move the object? Does it smoothly correct the position?

Just to keep xPos in a reasonable range I suggest to change the moveServo() function like this

void moveServo(int delta){
  xPos += delta;
  servoPosition = constrain(xPos,0,120);
  servo.write(servoPosition);
  if (xPos <   0.0) {xPos =   0.0;}
  if (xPos > 120.0) {xPos = 120.0;}
}

I assume that you have to test the sketch now for a while to be sure that no unexpected "features" may arise.

A further part to take care of is this

  for (int i = 0; i < huskylens.countBlocks(); i++) {
    HUSKYLENSResult result = huskylens.getBlock(i);

If the Huskylens detects more than one block (more than one ID) it will start to move the servo in different directions every 250 ms .... If it is always ID == 2 you want to follow, you could either use an if clause to only follow ID 2 or only retrieve data for ID 2 from the device.

You may try also this one that includes both changes:

/***********************************************
*  HUSKYLENS face recog PAN (servo)  *
*  Author:                                                  *
*  Date:06/10/24 - working                        *
*  Camera protocol = I2C                          *
*  Target Micro - Uno                                 *
 ***********************************************/
/*

 2024/01/13
  
 Compiled but not tested ...
 ec2021

*/

#include <HUSKYLENS.h>
#include <Wire.h>
#include <Servo.h>

HUSKYLENS huskylens;
Servo servo;

int servoPosition = 90;  // Initial position
float xPos = 90.0;

void setup() {
  servo.attach(9);  // Connect the servo to pin 9
  servo.write(servoPosition);
  Serial.begin(115200);
  Wire.begin();
  while (!huskylens.begin(Wire)) {
    Serial.println(F("HUSKYLENS not connected!"));
    delay(100);
  }
  huskylens.writeAlgorithm(ALGORITHM_FACE_RECOGNITION);
}

void loop() {
  if (!huskylens.request()) return;

  for (int i = 0; i < huskylens.countBlocks(); i++) {
    HUSKYLENSResult result = huskylens.getBlock(i);
    int x_center = result.xCenter;
    int width = result.width;
    int y_center = result.yCenter;
    int height = result.height;
    int ID = result.ID;
    Serial.print("Object tracked at X: ");
    Serial.print(x_center);
    Serial.print(", Y: ");
    Serial.print(y_center);
    Serial.print(", Width: ");
    Serial.print(width);
    Serial.print(", Height: ");
    Serial.println(height);
    Serial.print("Tracked ID: ");
    Serial.println(ID);
    if (ID == 2) {
      byte mode = 0;
      if (x_center > 10 && x_center < 130) { mode = 1; }
      if (x_center > 190) { mode = 2; }
      switch (mode) {
        case 0:  // No change with x_center below 10 or between 130 and 190
          break;
        case 1:  // Increase servo angle
          moveServo(1.5);
          break;
        case 2:  // Reduce servo angle
          moveServo(-1.5);
          break;
      }
    }
    delay(250);  // Add a delay to allow the servo to move to the new position
  }
}

void moveServo(int delta) {
  xPos += delta;
  servoPosition = constrain(xPos, 0, 120);
  servo.write(servoPosition);
  if (xPos < 0.0) { xPos = 0.0; }
  if (xPos > 120.0) { xPos = 120.0; }
}

Good luck and have fun with your Huskylens!
ec2021

And if you feel your problem was solved just mark the thread as "solved" ... :wink:

1 Like

Thanks, I going out now but will resume tomorrow - the y (horiz) axis is stable because the y servo is not connected yet, I wanted to get the x coordinate working first! I still want to thank you for all your great help, I purchased a number of HuskyLens devices and have the pro versions with the better camera Gravity: HUSKYLENS PRO, an AI Machine Vision Sensor - DFRobot Wiki I would be really happy to send you one in order to show my appreciation! Best Regards Tony

I really appreciate your suggestion. Please don't think of me as ungrateful but I am more than satisfied that your problem is (or at least seems to be) solved :wink:

As an additional gift feel free to check this out:

/*

**********************************************

 2024/01/13
  
 Compiled but not tested ...
 ec2021

***********************************************

*/

#include <HUSKYLENS.h>
#include <Wire.h>
#include <Servo.h>

struct servoType {
  Servo servo;
  int position;
  float fPos;
  float delta;
  void init(byte aPin, int startPos, int aDelta) {
    servo.attach(aPin);
    position = startPos;
    fPos = startPos;
    delta = aDelta;
    servo.write(position);
  }
  void movePlus() {
    move(1);
  }
  void moveMinus() {
    move(-1);
  }
  void move(int dir) {
    fPos += dir * delta;
    position = constrain(fPos, 0, 120);
    servo.write(position);
    if (fPos < 0.0) { fPos = 0.0; }
    if (fPos > 120.0) { fPos = 120.0; }
  }
};

HUSKYLENS huskylens;
servoType horizontal;
servoType vertical;


void setup() {
  horizontal.init( 9, 90, 1.5);  // Connect the servo to pin  9, set start angle to 90, set Delta to 1.5, move servo to start angle
  vertical.init  (10, 90, 1.0);  // Connect the servo to pin 10, set start angle to 90, set Delta to 1.0, move servo to start angle
  Serial.begin(115200);
  Wire.begin();
  while (!huskylens.begin(Wire)) {
    Serial.println(F("HUSKYLENS not connected!"));
    delay(100);
  }
  huskylens.writeAlgorithm(ALGORITHM_FACE_RECOGNITION);
}

void loop() {
  if (!huskylens.request()) return;

  for (int i = 0; i < huskylens.countBlocks(); i++) {
    HUSKYLENSResult result = huskylens.getBlock(i);
    printResult(result);
    if (result.ID == 2) {
      handlePan(result.xCenter);
      handleTilt(result.yCenter);
    }
    delay(250);  // Add a delay to allow the servo to move to the new position
  }
}

void handlePan(int xCenter) {
  byte mode = 0;
  if (xCenter > 10 && xCenter < 130) { mode = 1; }
  if (xCenter > 190) { mode = 2; }
  switch (mode) {
    case 0:  // No change with x_center below 10 or between 130 and 190
      break;
    case 1:  // Increase servo angle
      horizontal.movePlus();
      break;
    case 2:  // Decrease servo angle
      horizontal.moveMinus();
      break;
  }
}

void handleTilt(int yCenter) {
  byte mode = 0;
  if (yCenter > 10 && yCenter < 90) { mode = 1; }
  if (yCenter > 150) { mode = 2; }
  switch (mode) {
    case 0:  // No change with yCenter below 10 or between 90 and 150
      break;
    case 1:  // Decrease servo angle
      vertical.moveMinus();
      break;
    case 2:  // Increase servo angle
      vertical.movePlus();
      break;
  }
}

void printResult(HUSKYLENSResult &Result) {
  Serial.print("Object tracked at X: ");
  Serial.print(Result.xCenter);
  Serial.print(", Y: ");
  Serial.print(Result.yCenter);
  Serial.print(", Width: ");
  Serial.print(Result.width);
  Serial.print(", Height: ");
  Serial.print(Result.height);
  Serial.print(", Tracked ID: ");
  Serial.println(Result.ID);
}

I just coded and compiled it, it is hopefully taking care of both pan and tilt (within the limits as in the example code you posted).

Please check the vertical servo pin (I just used pin 10 in the sketch, that might be wrong) and also the direction moving up or down. You can easily change that if your application works in the opposite directions.

Do not hesitate to post again if you encounter problems with the sketch; I cannot be sure it works as intended ...

Hope you have fun with your device!

Best regards from Germany to the UK!
ec2021

P.S.: There are still some "magic numbers" in the code so there is still room for improvement :wink:

1 Like

Hi, Firstly thank you so much for adding the y element! I have been trying it out, but the y servo is going the wrong way? I put the y servo on pin 10 - It is sort of working but the y does not center properly - it must be the y values? I will continue to play! Best Regards Tony

This is the function that handles the tilt angle. If it goes the wrong way we have to change from moveMinus() to movePlus() in case 1 and vice versa in case 2 like done here:

void handleTilt(int yCenter) {
  byte mode = 0;
  if (yCenter > 10 && yCenter < 90) { mode = 1; }
  if (yCenter > 150) { mode = 2; }
  switch (mode) {
    case 0:  // No change with yCenter below 10 or between 90 and 150
      break;
    case 1:  // Increase servo angle
      vertical.movePlus();
      break;
    case 2:  // Decrease servo angle
      vertical.moveMinus();
      break;
  }
}

It depends on how the servo is mounted to the Huskylens (how are the angles zero and 180 degrees related to the y coordinates).

With Pan the x coordinates have the same order as the angles (lower x = lower angle, higher x = higher angle). And now the same applies to the tilt servo. It was different in the Pan/Tilt example above which I used as reference.

it works depending on position of target - I swapped the servo direction as it was going the wrong way - from what I can see is that if "y" is between something like 115 and 125 (120 being "y" center) then the y servo should lock on target vertically - I tried to add these to your brilliant code with no luck so far?

I can still only guess ... :wink: So I'm afraid it is up to you to empirically find the right values ...

To ease that I replaced the "magic numbers" by constants which you can change in the header of the sketch:

/*

**********************************************

 2024/01/14
  
 Compiled but not tested ...
 ec2021

***********************************************

*/

#include <HUSKYLENS.h>
#include <Wire.h>
#include <Servo.h>

const int minAngleHor = 10;
const int lowAngleHor = 130;
const int highAngleHor = 190;
const int startAngleHor = 90;
const int maxServoHor = 120;
const float deltaHor    = 1.5;
const byte servoPinHor  = 9;

const int minAngleVert = 10;
const int lowAngleVert = 90;
const int highAngleVert = 150;
const int startAngleVert = 120;
const int maxServoVert = 120;
const float deltaVert    = 1.5;
const byte servoPinVert  = 10;

const int trackID = 2;


struct servoType {
  Servo servo;
  int position;
  float fPos;
  float delta;
  int maxServoAngle;
  void init(byte aPin, int startPos, int aDelta, int MaxServoAngle) {
    servo.attach(aPin);
    position = startPos;
    fPos = startPos;
    delta = aDelta;
    maxServoAngle = MaxServoAngle;
    servo.write(position);
  }
  void movePlus() {
    move(1);
  }
  void moveMinus() {
    move(-1);
  }
  void move(int dir) {
    fPos += dir * delta;
    position = constrain(fPos, 0, maxServoAngle);
    servo.write(position);
    if (fPos < 0.0) { fPos = 0.0; }
    if (fPos > maxServoAngle) { fPos = maxServoAngle; }
  }
};

HUSKYLENS huskylens;
servoType horizontal;
servoType vertical;

void setup() {
  horizontal.init(servoPinHor, startAngleHor, deltaHor, maxServoHor);   // Connect the servo to pin , set start angle, set Delta, set max Servo Angle, move servo to start angle
  vertical.init(servoPinVert, startAngleVert, deltaVert, maxServoVert);  // Connect the servo to pin, set start angle , set Delta,set max Servo Angle, move servo to start angle
  Serial.begin(115200);
  Wire.begin();
  while (!huskylens.begin(Wire)) {
    Serial.println(F("HUSKYLENS not connected!"));
    delay(100);
  }
  huskylens.writeAlgorithm(ALGORITHM_FACE_RECOGNITION);
}

void loop() {
  if (!huskylens.request()) return;

  for (int i = 0; i < huskylens.countBlocks(); i++) {
    HUSKYLENSResult result = huskylens.getBlock(i);
    printResult(result);
    if (result.ID == trackID) {
      handlePan(result.xCenter);
      handleTilt(result.yCenter);
    }
    delay(250);  // Add a delay to allow the servo to move to the new position
  }
}

void handlePan(int xCenter) {
  byte mode = 0;
  if (xCenter > minAngleHor && xCenter < lowAngleHor) { mode = 1; }
  if (xCenter > highAngleHor) { mode = 2; }
  switch (mode) {
    case 0:  // No change with x_center below minAngleHor or between lowAngleHor and highAngleHor
      break;
    case 1:  // Increase servo angle
      horizontal.movePlus();
      break;
    case 2:  // Decrease servo angle
      horizontal.moveMinus();
      break;
  }
}

void handleTilt(int yCenter) {
  byte mode = 0;
  if (yCenter > minAngleVert && yCenter < lowAngleVert) { mode = 1; }
  if (yCenter > highAngleVert) { mode = 2; }
  switch (mode) {
    case 0:  // No change with yCenter below 10 or between 90 and 150
      break;
    case 1:  // Decrease servo angle
      vertical.moveMinus();
      break;
    case 2:  // Increase servo angle
      vertical.movePlus();
      break;
  }
}

void printResult(HUSKYLENSResult &Result) {
  Serial.print("Object tracked at X: ");
  Serial.print(Result.xCenter);
  Serial.print(", Y: ");
  Serial.print(Result.yCenter);
  Serial.print(", Width: ");
  Serial.print(Result.width);
  Serial.print(", Height: ");
  Serial.print(Result.height);
  Serial.print(", Tracked ID: ");
  Serial.println(Result.ID);
}

Their meaning is as follows:

const int minAngleHor = 10;     // xCenter equal or less this will not lead to any movement
const int lowAngleHor = 130;   // xCenter higher than minAngleHor until here lead to servo movement
const int highAngleHor = 190; // xCenter higher than this will lead to servo movement

const int startAngleHor = 90;  // This is the start position of the horizontal servo
const int maxServoHor = 120; // This is the maximum angle the hor. servo can move to
const float deltaHor    = 1.5;    // Steps taken (in float) which are mapped to integers
const byte servoPinHor  = 9;  // The pin of the hor. servo

                                                        // And the similar constants for yCenter/vertical servo
const int minAngleVert = 10;
const int lowAngleVert = 90;
const int highAngleVert = 150;
const int startAngleVert = 120;
const int maxServoVert = 120;
const float deltaVert    = 1.5;
const byte servoPinVert  = 10;

const int trackID = 2;  // The ID that shall be used for servo control

If that's too tedious we could implement a serial input of e.g. lowAngleVert and highAngleVert so that you could change them "online" from the serial terminal ... Would that be required?

Good luck!

Hi I tried tried approach so the servo would stop moving vert between 115 and 125 but it doe not work? Thanks

void handleTilt(int yCenter) {
  byte mode = 0;

  if (yCenter > 115 && yCenter < 125) { mode = 0; }
  
  if (yCenter > 10 && yCenter < 90) { mode = 1; }

//  if (yCenter > 10 && yCenter > 110) { mode = 1; }

  if (yCenter > 150) { mode = 2; }

//  if (yCenter < 130) { mode = 2; }
  switch (mode) {
    case 0:  // No change with yCenter below 10 or between 90 and 150-------- >110 and <130
      break;
    case 1:  // Increase servo angle
      vertical.movePlus();
      break;
    case 2:  // Decrease servo angle
      vertical.moveMinus();
      break;
  }
}

Better use the sketch from post 34 and make these changes to the constants

const int minAngleVert = 10;

const int lowAngleVert =  115;  // from 90 to 115
const int highAngleVert = 125; // from 150 to 125

const int startAngleVert = 120;
const int maxServoVert = 120;
const float deltaVert    = 1.5;
const byte servoPinVert  = 10;

It will actually reduce the range where the servo does not move.

If you look at the code as it was before:

  • mode was set to zero
  • mode was set to 1 if yCenter is larger than 10 and less than 90
  • mode was set to 2 if yCenter was larger than 150

This illustrates the process (not scaled):
image

In case yCenter was between 115 and 125 mode was not changed at all, so it was already zero ...

  • Where mode is 0 the servo will not be moved.
  • mode 1 moves the servo in direction "plus" (larger angle)
  • mode 2 moves the servo in direction "minus" (lower angle)

P.S.: I must admit that minAngle, lowAngle etc. are a misleading names... I'd better used Pixel instead of Angle because those values relate to the x and y pixel coordinates from the Huskylens. I will fix that later.

1 Like

Hi, I am blown away by your generosity of help, it is really appreciated!
I am confused with the mode values? The latest code seems to only work fully in one direction tilt down works fine but tilt up moves but will not center?
I set the HL up and with the DFRobot basic code I get the following coordinates for the "y" axis - does this make sense to you for the "y" values? Best Regards Tony

Hi, I have changed the vars to my drawing values and it now works perfectly!!
Thank you so much for all your help!
Best Regards Tony

That's good news!

I cleaned up the sketch a little bit, especially the naming of constants and variables so that they should fit now better to their meaning... :wink:

Please check the constant values and change them to your data!

I have moved the delay(250) inside the if clause because it only makes sense when the target ID was detected. To speed up the sketch the delay could be made dependend on whether a servo moves or not ... if required ... :slight_smile:

Good luck and have fun!

/*

**********************************************

 2024/01/15
  
 Compiled but not tested ...
 ec2021

***********************************************

*/

#include <HUSKYLENS.h>
#include <Wire.h>
#include <Servo.h>

const int minPixelHor = 10;
const int lowPixelHor = 130;
const int highPixelHor = 190;
const int startAngleHor = 90;
const int maxServoHor = 120;
const float deltaHor    = 1.5;
const byte servoPinHor  = 9;

const int minPixelVert = 10;
const int lowPixelVert = 40;
const int highPixelVert = 190;
const int startAngleVert = 90;
const int maxServoVert = 120;
const float deltaVert    = 1.0;
const byte servoPinVert  = 10;

const int trackID = 2;


struct servoType {
  Servo servo;
  int angle;
  float fAngle;
  float delta;
  int maxServoAngle;
  void init(byte aPin, int startAngle, int aDelta, int MaxServoAngle) {
    servo.attach(aPin);
    angle = startAngle;
    fAngle = startAngle;
    delta = aDelta;
    maxServoAngle = MaxServoAngle;
    servo.write(angle);
  }
  void movePlus() {
    move(1);
  }
  void moveMinus() {
    move(-1);
  }
  void move(int dir) {
    fAngle += dir * delta;
    angle = constrain(fAngle, 0, maxServoAngle);
    servo.write(angle);
    if (fAngle < 0.0) { fAngle = 0.0; }
    if (fAngle > maxServoAngle) { fAngle = maxServoAngle; }
  }
};

HUSKYLENS huskylens;
servoType horizontal;
servoType vertical;

void setup() {
  horizontal.init(servoPinHor, startAngleHor, deltaHor, maxServoHor);   // Connect the servo to pin , set start angle, set Delta, set max Servo Angle, move servo to start angle
  vertical.init(servoPinVert, startAngleVert, deltaVert, maxServoVert);  // Connect the servo to pin, set start angle, set Delta,set max Servo Angle, move servo to start angle
  Serial.begin(115200);
  Wire.begin();
  while (!huskylens.begin(Wire)) {
    Serial.println(F("HUSKYLENS not connected!"));
    delay(100);
  }
  huskylens.writeAlgorithm(ALGORITHM_FACE_RECOGNITION);
}

void loop() {
  if (!huskylens.request()) return;

  for (int i = 0; i < huskylens.countBlocks(); i++) {
    HUSKYLENSResult result = huskylens.getBlock(i);
    printResult(result);
    if (result.ID == trackID) {
      handlePan(result.xCenter);
      handleTilt(result.yCenter);
      delay(250);  // Add a delay to allow the servo to move to the new position
    }
  }
}

void handlePan(int xCenter) {
  byte mode = 0;
  if (xCenter > minPixelHor && xCenter < lowPixelHor) { mode = 1; }
  if (xCenter > highPixelHor) { mode = 2; }
  switch (mode) {
    case 0:  // No change with x_center below minPixelHor or between lowPixelHor and highPixelHor
      break;
    case 1:  // Increase servo angle
      horizontal.movePlus();
      break;
    case 2:  // Decrease servo angle
      horizontal.moveMinus();
      break;
  }
}

void handleTilt(int yCenter) {
  byte mode = 0;
  if (yCenter > minPixelVert && yCenter < lowPixelVert) { mode = 1; }
  if (yCenter > highPixelVert) { mode = 2; }
  switch (mode) {
    case 0:  // No change with yCenter below 10 or between 90 and 150
      break;
    case 1:  // Decrease servo angle
      vertical.moveMinus();
      break;
    case 2:  // Increase servo angle
      vertical.movePlus();
      break;
  }
}

void printResult(HUSKYLENSResult &Result) {
  Serial.print("Object tracked at X: ");
  Serial.print(Result.xCenter);
  Serial.print(", Y: ");
  Serial.print(Result.yCenter);
  Serial.print(", Width: ");
  Serial.print(Result.width);
  Serial.print(", Height: ");
  Serial.print(Result.height);
  Serial.print(", Tracked ID: ");
  Serial.println(Result.ID);
}

Could not resist to include this function ... :wink:

Delay based on servo movement
/*

**********************************************

 2024/01/15

 delay depending on servo movement ...
  
 Compiled but not tested ...
 ec2021

***********************************************

*/

#include <HUSKYLENS.h>
#include <Wire.h>
#include <Servo.h>

const int minPixelHor = 10;
const int lowPixelHor = 130;
const int highPixelHor = 190;
const int startAngleHor = 90;
const int maxServoHor = 120;
const float deltaHor    = 1.5;
const byte servoPinHor  = 9;

const int minPixelVert = 10;
const int lowPixelVert = 40;
const int highPixelVert = 190;
const int startAngleVert = 90;
const int maxServoVert = 120;
const float deltaVert    = 1.0;
const byte servoPinVert  = 10;

const int trackID = 2;


struct servoType {
  Servo servo;
  int angle;
  float fAngle;
  float delta;
  int maxServoAngle;
  void init(byte aPin, int startAngle, int aDelta, int MaxServoAngle) {
    servo.attach(aPin);
    angle = startAngle;
    fAngle = startAngle;
    delta = aDelta;
    maxServoAngle = MaxServoAngle;
    servo.write(angle);
  }
  void movePlus() {
    move(1);
  }
  void moveMinus() {
    move(-1);
  }
  void move(int dir) {
    fAngle += dir * delta;
    angle = constrain(fAngle, 0, maxServoAngle);
    servo.write(angle);
    if (fAngle < 0.0) { fAngle = 0.0; }
    if (fAngle > maxServoAngle) { fAngle = maxServoAngle; }
  }
};

HUSKYLENS huskylens;
servoType horizontal;
servoType vertical;

void setup() {
  horizontal.init(servoPinHor, startAngleHor, deltaHor, maxServoHor);   // Connect the servo to pin , set start angle, set Delta, set max Servo Angle, move servo to start angle
  vertical.init(servoPinVert, startAngleVert, deltaVert, maxServoVert);  // Connect the servo to pin, set start angle, set Delta,set max Servo Angle, move servo to start angle
  Serial.begin(115200);
  Wire.begin();
  while (!huskylens.begin(Wire)) {
    Serial.println(F("HUSKYLENS not connected!"));
    delay(100);
  }
  huskylens.writeAlgorithm(ALGORITHM_FACE_RECOGNITION);
}

void loop() {
  if (!huskylens.request()) return;

  for (int i = 0; i < huskylens.countBlocks(); i++) {
    HUSKYLENSResult result = huskylens.getBlock(i);
    printResult(result);
    if (result.ID == trackID) {
      if (handlePan(result.xCenter) || handleTilt(result.yCenter)){
        delay(250);  // Add a delay to allow the servo to move to the new position
      }
    }
  }
}

boolean handlePan(int xCenter) {
  byte mode = 0;
  if (xCenter > minPixelHor && xCenter < lowPixelHor) { mode = 1; }
  if (xCenter > highPixelHor) { mode = 2; }
  switch (mode) {
    case 0:  // No change with x_center below minPixelHor or between lowPixelHor and highPixelHor
      break;
    case 1:  // Increase servo angle
      horizontal.movePlus();
      break;
    case 2:  // Decrease servo angle
      horizontal.moveMinus();
      break;
  }
  return mode;  
}

boolean handleTilt(int yCenter) {
  byte mode = 0;
  if (yCenter > minPixelVert && yCenter < lowPixelVert) { mode = 1; }
  if (yCenter > highPixelVert) { mode = 2; }
  switch (mode) {
    case 0:  // No change with yCenter below 10 or between 90 and 150
      break;
    case 1:  // Decrease servo angle
      vertical.moveMinus();
      break;
    case 2:  // Increase servo angle
      vertical.movePlus();
      break;
  }
  return mode;
}

void printResult(HUSKYLENSResult &Result) {
  Serial.print("Object tracked at X: ");
  Serial.print(Result.xCenter);
  Serial.print(", Y: ");
  Serial.print(Result.yCenter);
  Serial.print(", Width: ");
  Serial.print(Result.width);
  Serial.print(", Height: ");
  Serial.print(Result.height);
  Serial.print(", Tracked ID: ");
  Serial.println(Result.ID);
}

Hi Thanks for this! I have been playing with the code and just noticed something - it will only track face 2 (ID.2) which is Mr Bean! All the other 11 faces get no tracking, I guess the code is set to only respond to face 2 (ID.2) - can it be set to follow any ID (face)? ID. is the recognised face's identity returned by HuskyLens.