Target Tracking software is working !!
williampretty 2026-04-28 06:40:02 26 Views0 Replies I got the software working now I need to know how to teach the HuskyLens 2 to recognise and track one of the built in objects.
Is there a wiki about this ?
Here is the open source code:
--------------------------------- Code ----------------------------
// Devastator_Robot_5.ino
#include <Servo.h>
#include <NewPing.h> //https://bitbucket.org/teckel12/arduino-new-ping/downloads/
#include "DFRobot_HuskylensV2.h"
#include "Wire.h"
// Create object
HuskylensV2 huskylens;
// #define TRIG_PIN A4 // Change to A2 to prevent conflict with Husky Lens camera
// #define ECHO_PIN A5 // Chamge to A3 to prevent conflict with Husky Lens camera
#define TRIG_PIN A2
#define ECHO_PIN A3
#define MAX_DISTANCE 200
int E1 = 5; //M1 Speed Control
int E2 = 6; //M2 Speed Control
int M1 = 4; //M1 Direction Control
int M2 = 7; //M1 Direction Control
char input;
bool flag = 0;
int centerThreshold = 50; // Tolerance for steering
int xCenter = 160;
int TargetX = 0;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
Servo myservo;
int distance = 100;
void setup(void) {
/* Setup baud rate for serial communication */
// Serial.begin(115200);
Serial.begin(9600);
delay(100);
myservo.attach(8);
myservo.write(120);
delay(2000);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
// Serial.print("Switching to tracking mode");
// Switch to Object Tracking mode
// huskylens.switchAlgorithm(ALGORITHM_OBJECT_TRACKING);
Serial.println("Hello World");
}
void scan() {
if (Serial.available() > 0) {
input = Serial.read();
/* Manual mode */
if (input == 'f' || input == 'b' || input == 'l' || input == 'r' || input == 's') {
manual();
}
/* Automatic mode */
else if (input == 'A') {
flag = 1;
automatic();
} else if (input == 'L') {
bootLeft();
} else if (input == 'R') {
bootRight();
} else if (input == 'C') {
camera();
}
}
}
void loop() {
scan();
}
/* Reading char 'M' on serial for stopping current mode */
void modeChange() {
if (Serial.available() > 0) {
input = Serial.read();
if (input == 'M') {
Serial.println("Changing Mode ");
flag = 0;
stop();
}
}
}
/*Turn 180 Right */
void bootRight() {
Serial.println("Hard right turn");
hardRight();
stop();
}
/*Turn 180 Left */
void bootLeft() {
Serial.println("Hard left turn");
hardLeft();
stop();
}
/********************************************* MANUAL MODE ********************************************/
void manual() {
if (input == 'f') {
moveForward();
} else if (input == 'b') {
moveBackward();
} else if (input == 'l') {
turnLeft();
} else if (input == 'r') {
turnRight();
} else if (input == 's') {
stop();
}
}
/********************************************* AUTOMATIC MODE ********************************************/
void automatic(void) {
Serial.println(distance);
Serial.println("Automatic");
int distanceR = 0;
int distanceL = 0;
delay(40);
while (flag == 1) {
moveForward();
modeChange();
distance = readPing();
delay(100);
if (distance <= 20) {
stop();
delay(100);
moveBackward();
delay(500);
stop();
delay(200);
distanceR = lookRight();
delay(200);
distanceL = lookLeft();
delay(200);
if (distanceR >= distanceL) {
Serial.println("Turning Right");
turnRight();
moveForward();
} else {
Serial.println("Turning Left");
turnLeft();
moveForward();
}
}
distance = readPing();
}
}
void turnRight() {
analogWrite(E1, 155);
analogWrite(E2, 155);
digitalWrite(M1, LOW);
digitalWrite(M2, HIGH);
delay(1200);
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
}
void hardRight() {
analogWrite(E1, 155);
analogWrite(E2, 155);
digitalWrite(M1, LOW);
digitalWrite(M2, HIGH);
delay(2400);
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
}
void turnLeft() {
analogWrite(E1, 155);
analogWrite(E2, 155);
digitalWrite(M1, HIGH);
digitalWrite(M2, LOW);
delay(1200);
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
}
void hardLeft() {
analogWrite(E1, 155);
analogWrite(E2, 155);
digitalWrite(M1, HIGH);
digitalWrite(M2, LOW);
delay(2400);
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
}
void stop(void) //Stop
{
Serial.println("Stoping");
digitalWrite(E1, 0);
digitalWrite(M1, LOW);
digitalWrite(E2, 0);
digitalWrite(M2, LOW);
}
void moveBackward() {
Serial.println("Moving backwards");
digitalWrite(M1, LOW);
digitalWrite(M2, LOW);
analogWrite(E2, 200);
analogWrite(E1, 200);
delay(5);
}
void moveForward() {
Serial.println("Moving forwards");
digitalWrite(M1, HIGH);
digitalWrite(M2, HIGH);
analogWrite(E2, 200); //PWM Speed Control
analogWrite(E1, 200);
delay(5);
}
int lookRight() {
myservo.write(50);
delay(500);
int distance = readPing();
delay(100);
myservo.write(120);
Serial.print("Looking right. Distance:");
Serial.println(distance);
return distance;
}
int lookLeft() {
myservo.write(170);
delay(500);
int distance = readPing();
delay(100);
myservo.write(120);
Serial.print("Looking left. Distance:");
Serial.println(distance);
return distance;
}
int readPing() {
delay(70);
int cm = sonar.ping_cm();
if (cm == 0) {
cm = 250;
}
return cm;
}
void camera() {
while (!huskylens.begin(Wire)) {
Serial.println("Waiting ... ");
Serial.println("Huskylens not found! Check wiring.");
delay(100);
}
// Switch to Object Tracking mode
huskylens.switchAlgorithm(ALGORITHM_OBJECT_TRACKING);
Serial.println("Switching to tracking mode");
while (true) {
// Request results from the sensor
huskylens.getResult(ALGORITHM_OBJECT_TRACKING);
if (huskylens.available(ALGORITHM_OBJECT_TRACKING)) {
// Serial.println("Tracking ...");
// Get the object closest to the center crosshair
auto target = huskylens.getCachedCenterResult(ALGORITHM_OBJECT_TRACKING);
// Check if object is left, right, or center
TargetX = (RET_ITEM_NUM(target, Result, xCenter));
Serial.print (TargetX);
if (TargetX < (160 - centerThreshold)) {
//Object on left: Turn Left
Serial.println(" Turning Left");
turnLeft();
delay(1000);
stop();
} else if (TargetX > (160 + centerThreshold)) {
//Object on right: Turn Right
Serial.println(" Turning Right");
turnRight();
delay(1000);
stop();
}
else {
Serial.println(" Moving Forward");
moveForward();
delay(1000);
stop();
}
}
delay(1500);
}
}
