I wonder if anyone can give me some help/advice as I am new to Arduino code and am struggling to find a solution to the camera/servo jogging (slightly) back and forth when looking at a target. The cause is Huskylens outputs the x coordinate +- 6 positions from the centre variable. I think that some sort of averaging would help as the data comes in from the camera. Maybe just accumulating 10 samples and / by 5 may help, a median filter would be best, but this is past my current Arduino coding abilities. Below is the code, any help would be greatly appreciated from you Arduino experts! Thanks for reading.
/***********************************************
* HUSKYLENS face recog PAN (servo) *
* Author: *
* Date:06/10/24 - working *
* Camera protocol = I2C *
* Target Micro - Uno *
***********************************************/
#include <HUSKYLENS.h>
#include <Wire.h>
#include <Servo.h>
HUSKYLENS huskylens;
Servo servo;
int lastServoPosition = 90; // Initial position
int threshold = 1; // Set a threshold for position changes
void setup() {
servo.attach(9); // Connect the servo to pin 9
servo.write(90);
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);
delay(100);
// Map the x-coordinate to the servo range (0-180 degrees)
int servoPosition = map(x_center, 0, 320, 180, 0);
// Calculate the adjustment needed
int servoAdjustment = servoPosition - lastServoPosition;
// Limit the adjustment to prevent the servo from moving too quickly
if (servoAdjustment > threshold) {
servoAdjustment = threshold;
} else if (servoAdjustment < -threshold) {
servoAdjustment = -threshold;
}
// Adjust the servo position and update the last position
servo.attach(9);
servo.write(lastServoPosition + servoAdjustment);
lastServoPosition += servoAdjustment;
delay(150); // Add a delay to allow the servo to move to the new position
//servo.detach();
}
}
