Hardware components | ||||||
| × | 1 | ||||
| × | 1 | ||||
Software apps and online services | ||||||
 |
| |||||
Hand tools and fabrication machines | ||||||
 |
| |||||
 |
| |||||
In one of my previous videos, I presented an interactive Rock-Paper-Scissors project in which we compete against the myCobot 280 M5Stack, a 6-DOF robotic arm from Elephant Robotics. In that video, I also explained some of the basic features of the robot and the installation of the required software support.
For this project, we will again use the M5Stack and MyCobotBasic libraries, which are listed at the end of the text, together with Python 3.11.
Face detection is performed using the MediaPipe Face Detection machine-learning model, while OpenCV handles the camera stream, graphical visualization, and calculation of the face position inside the image. PySerial is used for communication between the Python application and the M5Stack controller.
These Python packages can be installed on Windows 11 by running the following command in Command Prompt or Windows PowerShell:
pip install mediapipe opencv-python pyserial.
This time, the goal is quite different. The myCobot 280 robotic arm will first detect a human face using the Camera Flange 2.0 module, also manufactured by Elephant Robotics, and then automatically move the camera so that the detected face remains near the center of the image.
This project is sponsored by PCBWay . From September 1, 2026, to January 24, 2027, PCBWay is hosting its 9th Project Design Contest. You can enter in five different categories: Edge AI & IoT, Robotics, Power, Open Theme, and Mechanical. Valuable prizes—including cash, coupons, and single-board computers—await the winners, while every participant will receive a Raspberry Pi Pico. Don't miss this opportunity to share your work and win great prizes. Make PCBWay your first choice!
The project is divided into two main software parts: a Python program running on the PC and an Arduino C++ program running on the M5Stack controller of the robot. The Python program is responsible for computer vision, and depending on the position of the face it sends simple commands to the M5Stack through the USB serial connection: Now the M5 Stack controls the servos and moves the arm along with the camera. For smooth movement, the Arduino program uses small incremental joint movements rather than continuously sending large absolute position changes.
During development, I tested two different methods of moving the camera horizontally.
- In the first version: J5 controls LEFT and RIGHT movement, J4 controls UP and DOWN movement. This gives a compact tracking movement.
- In the second version: J1 controls LEFT and RIGHT movement, J4 controls UP and DOWN movement. In this case, the complete upper part of the robotic arm rotates around the base. Visually, this movement looks more like a robot turning its “head” toward the person, and it gives the project a very different character.
Both versions use exactly the same Python face-tracking program.
The small M5Stack display is used as a local status monitor for the complete system: Under the Title the current operating state is displayed, then horizontal and vertical positions numerically in degrees. In addition to the numerical values, two graphical position bars are displayed, for horisontal and vertical movements.
The three buttons on the M5Stack also have dedicated functions. Button A is used for Center, Button B activates the complete Home position at start and Button C switches the sound indication ON or OFF.
The OpenCV camera window provides additional visual information that is very useful during testing and calibration.
The built-in 5×5 LED panel on the myCobot is used as another simple but very effective status indicator. Different colors represent different operating conditions. Blue means that the robot is in a neutral or waiting condition. Green means that a face has been detected and the robot is actively following its movement. Red, the detected face is inside the central stop zone and no further movement is currently required. Yellow means that a previously tracked face has genuinely disappeared from the camera view for approximately one second.
Several short sound signals are also used to make the operating states easier to recognize without looking at the display. HOME uses the same short sound that I used in my previous Rock-Paper-Scissors project. CENTER and LOCKED use a short 1800 Hz beep and a longer low-frequency tone indicates that the tracked face has been lost. The complete sound system can be enabled or disabled at any time using button C.
Now let's see how the device works in real conditions. By pressing the Home button, the entire arm is placed in a position ready for tracking. Center returns only the two tracking joints to their center positions. For a clearer presentation I am using a printed face from the "this person does not exist" website.
And finallu a short conclusion. This project transforms the myCobot 280 into a smooth real-time 2D face-tracking robot using Python, MediaPipe, OpenCV, and Arduino. Two different joint-control methods demonstrate how the same vision system can produce very different and natural-looking robotic movements.
// by mircemk October, 2026
#include <M5Stack.h>
#include <MyCobotBasic.h>
MyCobotBasic myCobot;
// =====================================================
// VERSION
// =====================================================
// TEST 8H
// J1 = Horizontal tracking
// J4 = Vertical tracking
// Added graphical HOR / VERT position bars
// =====================================================
// =====================================================
// TRACKING SETTINGS
// =====================================================
#define J1_STEP 1.4f
#define J4_STEP 1.4f
// Horizontal tracking with J1
#define J1_MIN -60.0f
#define J1_MAX 60.0f
// Vertical +/-30 deg around J4 = 90 deg
#define J4_MIN 60.0f
#define J4_MAX 120.0f
#define JOG_SPEED 100
#define HOME_SPEED 60
#define JOINT_INTERVAL_MS 40
// =====================================================
// TRACKING HOME POSITION
// =====================================================
#define J1_HOME 0
#define J2_HOME 0
#define J3_HOME -90
#define J4_HOME 90
#define J5_HOME 0
#define J6_HOME -45
// =====================================================
// SOUND
// =====================================================
#define SPEAKER_PIN 25
#define DAC_VOL_HI 160
#define DAC_VOL_LO 0
bool soundEnabled = true;
// =====================================================
// CURRENT POSITION
// =====================================================
float j1Position = J1_HOME;
float j4Position = J4_HOME;
unsigned long lastJ1MoveTime = 0;
unsigned long lastJ4MoveTime = 0;
// =====================================================
// STATUS
// =====================================================
String currentState = "READY";
String lastTrackingStatus = "WAITING";
bool manualMoveActive = false;
// =====================================================
// GRAPHICAL POSITION BARS
// =====================================================
// -----------------------------------------------------
// VERTICAL BAR - LEFT SIDE
// -----------------------------------------------------
// -----------------------------------------------------
// VERTICAL BAR - LEFT SIDE
// -----------------------------------------------------
#define VBAR_X 8
#define VBAR_Y 10
#define VBAR_W 12
#define VBAR_H 204
// -----------------------------------------------------
// HORIZONTAL BAR - BOTTOM
// -----------------------------------------------------
#define HBAR_X 58
#define HBAR_Y 222
#define HBAR_W 245
#define HBAR_H 12
// =====================================================
// 5x5 RGB LED
// =====================================================
void setStatusLED(int r, int g, int b)
{
myCobot.setLEDRGB(r, g, b);
}
void setBlueLED()
{
setStatusLED(0, 0, 255);
}
void setGreenLED()
{
setStatusLED(0, 255, 0);
}
void setRedLED()
{
setStatusLED(255, 0, 0);
}
void setYellowLED()
{
setStatusLED(255, 255, 0);
}
// =====================================================
// SPEAKER
// =====================================================
void initSpeaker()
{
pinMode(SPEAKER_PIN, OUTPUT);
dacWrite(
SPEAKER_PIN,
DAC_VOL_LO
);
}
void stopTone()
{
dacWrite(
SPEAKER_PIN,
DAC_VOL_LO
);
}
void playTone(
int freq,
int duration_ms
)
{
if (!soundEnabled)
{
return;
}
if (
freq <= 0
||
duration_ms <= 0
)
{
return;
}
const uint32_t total_us =
(uint32_t)duration_ms * 1000UL;
const uint32_t half_period_us =
500000UL / (uint32_t)freq;
if (half_period_us == 0)
{
return;
}
const uint32_t startTime =
micros();
while (
micros() - startTime
<
total_us
)
{
dacWrite(
SPEAKER_PIN,
DAC_VOL_HI
);
delayMicroseconds(
half_period_us
);
dacWrite(
SPEAKER_PIN,
DAC_VOL_LO
);
delayMicroseconds(
half_period_us
);
yield();
}
stopTone();
}
// =====================================================
// SOUND EFFECTS
// =====================================================
// HOME sound from original RPS project
void homeSound()
{
playTone(
700,
60
);
}
// CENTER / LOCKED
void centerSound()
{
playTone(
1800,
80
);
}
// REAL FACE LOST
void noFaceSound()
{
playTone(
320,
450
);
}
// =====================================================
// STATIC BAR FRAMES
// =====================================================
void drawBarFrames()
{
// ---------------------------------------------------
// VERTICAL BAR
// ---------------------------------------------------
M5.Lcd.drawRect(
VBAR_X,
VBAR_Y,
VBAR_W,
VBAR_H,
WHITE
);
int verticalCenter =
VBAR_Y + VBAR_H / 2;
M5.Lcd.drawFastHLine(
VBAR_X,
verticalCenter,
VBAR_W,
WHITE
);
// ---------------------------------------------------
// HORIZONTAL BAR
// ---------------------------------------------------
M5.Lcd.drawRect(
HBAR_X,
HBAR_Y,
HBAR_W,
HBAR_H,
WHITE
);
int horizontalCenter =
HBAR_X + HBAR_W / 2;
M5.Lcd.drawFastVLine(
horizontalCenter,
HBAR_Y,
HBAR_H,
WHITE
);
}
// =====================================================
// HORIZONTAL POSITION BAR
// J1 = -60 ... 0 ... +60
// =====================================================
void updateHorizontalBar()
{
// Clear inside only.
M5.Lcd.fillRect(
HBAR_X + 1,
HBAR_Y + 1,
HBAR_W - 2,
HBAR_H - 2,
BLACK
);
int centerX =
HBAR_X + HBAR_W / 2;
int insideHeight =
HBAR_H - 2;
// Available width from center
// to one end of the bar.
int halfWidth =
(HBAR_W - 3) / 2;
if (j1Position > 0.0f)
{
float ratio =
j1Position / J1_MAX;
if (ratio > 1.0f)
{
ratio = 1.0f;
}
int fillWidth =
(int)(
ratio * halfWidth
);
if (fillWidth > 0)
{
M5.Lcd.fillRect(
centerX + 1,
HBAR_Y + 1,
fillWidth,
insideHeight,
YELLOW
);
}
}
else if (j1Position < 0.0f)
{
float ratio =
(-j1Position) / (-J1_MIN);
if (ratio > 1.0f)
{
ratio = 1.0f;
}
int fillWidth =
(int)(
ratio * halfWidth
);
if (fillWidth > 0)
{
M5.Lcd.fillRect(
centerX - fillWidth,
HBAR_Y + 1,
fillWidth,
insideHeight,
YELLOW
);
}
}
// Redraw center zero line.
M5.Lcd.drawFastVLine(
centerX,
HBAR_Y,
HBAR_H,
WHITE
);
// Redraw frame.
M5.Lcd.drawRect(
HBAR_X,
HBAR_Y,
HBAR_W,
HBAR_H,
WHITE
);
}
// =====================================================
// VERTICAL POSITION BAR
// J4 relative position = -30 ... 0 ... +30
// =====================================================
void updateVerticalBar()
{
// Clear inside only.
M5.Lcd.fillRect(
VBAR_X + 1,
VBAR_Y + 1,
VBAR_W - 2,
VBAR_H - 2,
BLACK
);
int centerY =
VBAR_Y + VBAR_H / 2;
int insideWidth =
VBAR_W - 2;
int halfHeight =
(VBAR_H - 3) / 2;
float verticalPosition =
j4Position - J4_HOME;
// ---------------------------------------------------
// POSITIVE = UP
// ---------------------------------------------------
if (verticalPosition > 0.0f)
{
float maxPositive =
J4_MAX - J4_HOME;
float ratio =
verticalPosition /
maxPositive;
if (ratio > 1.0f)
{
ratio = 1.0f;
}
int fillHeight =
(int)(
ratio * halfHeight
);
if (fillHeight > 0)
{
M5.Lcd.fillRect(
VBAR_X + 1,
centerY - fillHeight,
insideWidth,
fillHeight,
RED
);
}
}
// ---------------------------------------------------
// NEGATIVE = DOWN
// ---------------------------------------------------
else if (verticalPosition < 0.0f)
{
float maxNegative =
J4_HOME - J4_MIN;
float ratio =
(-verticalPosition) /
maxNegative;
if (ratio > 1.0f)
{
ratio = 1.0f;
}
int fillHeight =
(int)(
ratio * halfHeight
);
if (fillHeight > 0)
{
M5.Lcd.fillRect(
VBAR_X + 1,
centerY + 1,
insideWidth,
fillHeight,
RED
);
}
}
// Redraw center zero line.
M5.Lcd.drawFastHLine(
VBAR_X,
centerY,
VBAR_W,
WHITE
);
// Redraw frame.
M5.Lcd.drawRect(
VBAR_X,
VBAR_Y,
VBAR_W,
VBAR_H,
WHITE
);
}
// =====================================================
// UPDATE BOTH POSITION BARS
// =====================================================
void updatePositionBars()
{
updateHorizontalBar();
updateVerticalBar();
}
// =====================================================
// DISPLAY
// =====================================================
void showScreen()
{
M5.Lcd.fillScreen(
BLACK
);
// ---------------------------------------------------
// TITLE
// ---------------------------------------------------
M5.Lcd.setTextColor(
CYAN
);
M5.Lcd.setTextSize(
3
);
M5.Lcd.setCursor(
45,
10
);
M5.Lcd.println(
"2D TRACKING"
);
// ---------------------------------------------------
// STATUS
// ---------------------------------------------------
M5.Lcd.setTextSize(
2
);
M5.Lcd.setTextColor(
WHITE
);
M5.Lcd.setCursor(
32,
55
);
M5.Lcd.print(
"STATUS: "
);
M5.Lcd.setTextColor(
YELLOW
);
M5.Lcd.println(
currentState
);
// ---------------------------------------------------
// HOR
// ---------------------------------------------------
M5.Lcd.setTextColor(
WHITE
);
M5.Lcd.setCursor(
32,
95
);
M5.Lcd.print(
"HOR:"
);
M5.Lcd.setTextColor(
GREEN
);
M5.Lcd.setCursor(
105,
95
);
if (j1Position >= 0)
{
M5.Lcd.print("+");
}
M5.Lcd.print(
j1Position,
1
);
M5.Lcd.print(
" deg"
);
// ---------------------------------------------------
// VERT
// ---------------------------------------------------
float verticalPosition =
j4Position - J4_HOME;
M5.Lcd.setTextColor(
WHITE
);
M5.Lcd.setCursor(
32,
130
);
M5.Lcd.print(
"VERT:"
);
M5.Lcd.setTextColor(
GREEN
);
M5.Lcd.setCursor(
105,
130
);
if (verticalPosition >= 0)
{
M5.Lcd.print("+");
}
M5.Lcd.print(
verticalPosition,
1
);
M5.Lcd.print(
" deg"
);
// ---------------------------------------------------
// BUTTON A
// ---------------------------------------------------
M5.Lcd.setTextColor(
WHITE
);
M5.Lcd.setCursor(
32,
170
);
M5.Lcd.println(
"A: CENTER (J1/J4)"
);
// ---------------------------------------------------
// BUTTON B
// ---------------------------------------------------
M5.Lcd.setCursor(
32,
200
);
M5.Lcd.print(
"B: HOME"
);
// ---------------------------------------------------
// BUTTON C
// ---------------------------------------------------
M5.Lcd.setCursor(
130,
200
);
M5.Lcd.print(
"C: SOUND "
);
if (soundEnabled)
{
M5.Lcd.setTextColor(
GREEN
);
M5.Lcd.print(
"ON"
);
}
else
{
M5.Lcd.setTextColor(
RED
);
M5.Lcd.print(
"OFF"
);
}
// ---------------------------------------------------
// POSITION BAR FRAMES
// ---------------------------------------------------
drawBarFrames();
updatePositionBars();
}
// =====================================================
// UPDATE DISPLAY
// =====================================================
void updateDisplay()
{
// ---------------------------------------------------
// STATUS
// ---------------------------------------------------
M5.Lcd.fillRect(
115,
45,
200,
35,
BLACK
);
M5.Lcd.setTextColor(
YELLOW
);
M5.Lcd.setTextSize(
2
);
M5.Lcd.setCursor(
115,
55
);
M5.Lcd.print(
currentState
);
// ---------------------------------------------------
// HOR
// ---------------------------------------------------
M5.Lcd.fillRect(
105,
85,
180,
35,
BLACK
);
M5.Lcd.setTextColor(
GREEN
);
M5.Lcd.setCursor(
105,
95
);
if (j1Position >= 0)
{
M5.Lcd.print("+");
}
M5.Lcd.print(
j1Position,
1
);
M5.Lcd.print(
" deg"
);
// ---------------------------------------------------
// VERT
// ---------------------------------------------------
float verticalPosition =
j4Position - J4_HOME;
M5.Lcd.fillRect(
105,
120,
180,
35,
BLACK
);
M5.Lcd.setTextColor(
GREEN
);
M5.Lcd.setCursor(
105,
130
);
if (verticalPosition >= 0)
{
M5.Lcd.print("+");
}
M5.Lcd.print(
verticalPosition,
1
);
M5.Lcd.print(
" deg"
);
// ---------------------------------------------------
// SOUND
// ---------------------------------------------------
M5.Lcd.fillRect(
238,
190,
80,
30,
BLACK
);
M5.Lcd.setCursor(
238,
200
);
if (soundEnabled)
{
M5.Lcd.setTextColor(
GREEN
);
M5.Lcd.print(
"ON"
);
}
else
{
M5.Lcd.setTextColor(
RED
);
M5.Lcd.print(
"OFF"
);
}
// ---------------------------------------------------
// GRAPHICAL BARS
// ---------------------------------------------------
updatePositionBars();
}
// =====================================================
// JOG_INCREMENT
// =====================================================
void jogIncrement(
byte joint,
float increment,
...
This file has been truncated, please download it to see its full contents.
// by mircemk October, 2026
#include <M5Stack.h>
#include <MyCobotBasic.h>
MyCobotBasic myCobot;
// =====================================================
// TRACKING SETTINGS
// =====================================================
#define J4_STEP 1.4f
#define J5_STEP 1.4f
// Vertical range = +/-30 deg around J4 = 90
#define J4_MIN 60.0f
#define J4_MAX 120.0f
// Horizontal range = +/-50 deg
#define J5_MIN -50.0f
#define J5_MAX 50.0f
#define JOG_SPEED 100
#define HOME_SPEED 60
#define JOINT_INTERVAL_MS 40
// =====================================================
// TRACKING HOME POSITION
// =====================================================
#define J1_HOME 0
#define J2_HOME 0
#define J3_HOME -90
#define J4_HOME 90
#define J5_HOME 0
#define J6_HOME -45
// =====================================================
// SOUND
// =====================================================
#define SPEAKER_PIN 25
#define DAC_VOL_HI 160
#define DAC_VOL_LO 0
bool soundEnabled = true;
// =====================================================
// POSITION / STATUS
// =====================================================
float j4Position = J4_HOME;
float j5Position = J5_HOME;
unsigned long lastJ4MoveTime = 0;
unsigned long lastJ5MoveTime = 0;
String currentState = "READY";
String lastTrackingStatus = "WAITING";
bool manualMoveActive = false;
// =====================================================
// GRAPHICAL POSITION BARS
// =====================================================
// -----------------------------------------------------
// VERTICAL BAR - LEFT
// Long visual bar
// -----------------------------------------------------
#define VBAR_X 8
#define VBAR_Y 10
#define VBAR_W 16
#define VBAR_H 204
// -----------------------------------------------------
// HORIZONTAL BAR - BOTTOM
// -----------------------------------------------------
#define HBAR_X 58
#define HBAR_Y 222
#define HBAR_W 245
#define HBAR_H 12
// =====================================================
// 5x5 RGB LED
// =====================================================
void setStatusLED(int r, int g, int b)
{
myCobot.setLEDRGB(r, g, b);
}
void setBlueLED()
{
setStatusLED(0, 0, 255);
}
void setGreenLED()
{
setStatusLED(0, 255, 0);
}
void setRedLED()
{
setStatusLED(255, 0, 0);
}
void setYellowLED()
{
setStatusLED(255, 255, 0);
}
// =====================================================
// SPEAKER
// =====================================================
void initSpeaker()
{
pinMode(SPEAKER_PIN, OUTPUT);
dacWrite(SPEAKER_PIN, DAC_VOL_LO);
}
void stopTone()
{
dacWrite(SPEAKER_PIN, DAC_VOL_LO);
}
void playTone(int freq, int duration_ms)
{
if (!soundEnabled)
{
return;
}
if (freq <= 0 || duration_ms <= 0)
{
return;
}
const uint32_t total_us =
(uint32_t)duration_ms * 1000UL;
const uint32_t half_period_us =
500000UL / (uint32_t)freq;
if (half_period_us == 0)
{
return;
}
const uint32_t startTime = micros();
while (
micros() - startTime
<
total_us
)
{
dacWrite(
SPEAKER_PIN,
DAC_VOL_HI
);
delayMicroseconds(
half_period_us
);
dacWrite(
SPEAKER_PIN,
DAC_VOL_LO
);
delayMicroseconds(
half_period_us
);
yield();
}
stopTone();
}
// =====================================================
// SOUND EFFECTS
// =====================================================
void homeSound()
{
playTone(
700,
60
);
}
void centerSound()
{
playTone(
1800,
80
);
}
void noFaceSound()
{
playTone(
320,
450
);
}
// =====================================================
// BAR FRAMES
// =====================================================
void drawBarFrames()
{
// ---------------------------------------------------
// VERTICAL BAR
// ---------------------------------------------------
M5.Lcd.drawRect(
VBAR_X,
VBAR_Y,
VBAR_W,
VBAR_H,
WHITE
);
int verticalCenter =
VBAR_Y + VBAR_H / 2;
M5.Lcd.drawFastHLine(
VBAR_X,
verticalCenter,
VBAR_W,
WHITE
);
// ---------------------------------------------------
// HORIZONTAL BAR
// ---------------------------------------------------
M5.Lcd.drawRect(
HBAR_X,
HBAR_Y,
HBAR_W,
HBAR_H,
WHITE
);
int horizontalCenter =
HBAR_X + HBAR_W / 2;
M5.Lcd.drawFastVLine(
horizontalCenter,
HBAR_Y,
HBAR_H,
WHITE
);
}
// =====================================================
// HORIZONTAL BAR
// J5 = -50 ... 0 ... +50
// =====================================================
void updateHorizontalBar()
{
// Clear inside only
M5.Lcd.fillRect(
HBAR_X + 1,
HBAR_Y + 1,
HBAR_W - 2,
HBAR_H - 2,
BLACK
);
int centerX =
HBAR_X + HBAR_W / 2;
int insideHeight =
HBAR_H - 2;
int halfWidth =
(HBAR_W - 3) / 2;
// ---------------------------------------------------
// POSITIVE J5
// ---------------------------------------------------
if (j5Position > 0.0f)
{
float ratio =
j5Position / J5_MAX;
if (ratio > 1.0f)
{
ratio = 1.0f;
}
int fillWidth =
(int)(ratio * halfWidth);
if (fillWidth > 0)
{
M5.Lcd.fillRect(
centerX + 1,
HBAR_Y + 1,
fillWidth,
insideHeight,
YELLOW
);
}
}
// ---------------------------------------------------
// NEGATIVE J5
// ---------------------------------------------------
else if (j5Position < 0.0f)
{
float ratio =
(-j5Position) / (-J5_MIN);
if (ratio > 1.0f)
{
ratio = 1.0f;
}
int fillWidth =
(int)(ratio * halfWidth);
if (fillWidth > 0)
{
M5.Lcd.fillRect(
centerX - fillWidth,
HBAR_Y + 1,
fillWidth,
insideHeight,
YELLOW
);
}
}
// Zero line
M5.Lcd.drawFastVLine(
centerX,
HBAR_Y,
HBAR_H,
WHITE
);
// Frame
M5.Lcd.drawRect(
HBAR_X,
HBAR_Y,
HBAR_W,
HBAR_H,
WHITE
);
}
// =====================================================
// VERTICAL BAR
// J4 relative = -30 ... 0 ... +30
// =====================================================
void updateVerticalBar()
{
// Clear inside
M5.Lcd.fillRect(
VBAR_X + 1,
VBAR_Y + 1,
VBAR_W - 2,
VBAR_H - 2,
BLACK
);
int centerY =
VBAR_Y + VBAR_H / 2;
int insideWidth =
VBAR_W - 2;
int halfHeight =
(VBAR_H - 3) / 2;
float verticalPosition =
j4Position - J4_HOME;
// ---------------------------------------------------
// POSITIVE = UP
// ---------------------------------------------------
if (verticalPosition > 0.0f)
{
float maxPositive =
J4_MAX - J4_HOME;
float ratio =
verticalPosition /
maxPositive;
if (ratio > 1.0f)
{
ratio = 1.0f;
}
int fillHeight =
(int)(ratio * halfHeight);
if (fillHeight > 0)
{
M5.Lcd.fillRect(
VBAR_X + 1,
centerY - fillHeight,
insideWidth,
fillHeight,
RED
);
}
}
// ---------------------------------------------------
// NEGATIVE = DOWN
// ---------------------------------------------------
else if (verticalPosition < 0.0f)
{
float maxNegative =
J4_HOME - J4_MIN;
float ratio =
(-verticalPosition) /
maxNegative;
if (ratio > 1.0f)
{
ratio = 1.0f;
}
int fillHeight =
(int)(ratio * halfHeight);
if (fillHeight > 0)
{
M5.Lcd.fillRect(
VBAR_X + 1,
centerY + 1,
insideWidth,
fillHeight,
RED
);
}
}
// Zero line
M5.Lcd.drawFastHLine(
VBAR_X,
centerY,
VBAR_W,
WHITE
);
// Frame
M5.Lcd.drawRect(
VBAR_X,
VBAR_Y,
VBAR_W,
VBAR_H,
WHITE
);
}
// =====================================================
// UPDATE BOTH BARS
// =====================================================
void updatePositionBars()
{
updateHorizontalBar();
updateVerticalBar();
}
// =====================================================
// DISPLAY
// =====================================================
void showScreen()
{
M5.Lcd.fillScreen(BLACK);
// TITLE
M5.Lcd.setTextColor(CYAN);
M5.Lcd.setTextSize(3);
M5.Lcd.setCursor(45, 10);
M5.Lcd.println("2D TRACKING");
// STATUS
M5.Lcd.setTextSize(2);
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setCursor(32, 55);
M5.Lcd.print("STATUS: ");
M5.Lcd.setTextColor(YELLOW);
M5.Lcd.println(currentState);
// HOR
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setCursor(32, 95);
M5.Lcd.print("HOR:");
M5.Lcd.setTextColor(GREEN);
M5.Lcd.setCursor(105, 95);
if (j5Position >= 0)
{
M5.Lcd.print("+");
}
M5.Lcd.print(j5Position, 1);
M5.Lcd.print(" deg");
// VERT
float verticalPosition =
j4Position - J4_HOME;
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setCursor(32, 130);
M5.Lcd.print("VERT:");
M5.Lcd.setTextColor(GREEN);
M5.Lcd.setCursor(105, 130);
if (verticalPosition >= 0)
{
M5.Lcd.print("+");
}
M5.Lcd.print(verticalPosition, 1);
M5.Lcd.print(" deg");
// A
M5.Lcd.setTextColor(WHITE);
M5.Lcd.setCursor(32, 170);
M5.Lcd.println("A: CENTER (J4/J5)");
// B
M5.Lcd.setCursor(32, 200);
M5.Lcd.print("B: HOME");
// C
M5.Lcd.setCursor(130, 200);
M5.Lcd.print("C: SOUND ");
if (soundEnabled)
{
M5.Lcd.setTextColor(GREEN);
M5.Lcd.print("ON");
}
else
{
M5.Lcd.setTextColor(RED);
M5.Lcd.print("OFF");
}
// POSITION BARS
drawBarFrames();
updatePositionBars();
}
// =====================================================
// UPDATE DISPLAY
// =====================================================
void updateDisplay()
{
// STATUS
M5.Lcd.fillRect(
115,
45,
200,
35,
BLACK
);
M5.Lcd.setTextColor(YELLOW);
M5.Lcd.setTextSize(2);
M5.Lcd.setCursor(115, 55);
M5.Lcd.print(currentState);
// HOR
M5.Lcd.fillRect(
105,
85,
180,
35,
BLACK
);
M5.Lcd.setTextColor(GREEN);
M5.Lcd.setCursor(105, 95);
if (j5Position >= 0)
{
M5.Lcd.print("+");
}
M5.Lcd.print(j5Position, 1);
M5.Lcd.print(" deg");
// VERT
float verticalPosition =
j4Position - J4_HOME;
M5.Lcd.fillRect(
105,
120,
180,
35,
BLACK
);
M5.Lcd.setTextColor(GREEN);
M5.Lcd.setCursor(105, 130);
if (verticalPosition >= 0)
{
M5.Lcd.print("+");
}
M5.Lcd.print(verticalPosition, 1);
M5.Lcd.print(" deg");
// SOUND ON / OFF
M5.Lcd.fillRect(
238,
190,
80,
30,
BLACK
);
M5.Lcd.setCursor(238, 200);
if (soundEnabled)
{
M5.Lcd.setTextColor(GREEN);
M5.Lcd.print("ON");
}
else
{
M5.Lcd.setTextColor(RED);
M5.Lcd.print("OFF");
}
// POSITION BARS
updatePositionBars();
}
// =====================================================
// JOG_INCREMENT
// =====================================================
void jogIncrement(
byte joint,
float increment,
byte speed
)
{
int16_t incrementInt =
(int16_t)(increment * 100.0f);
byte angleHigh =
(byte)((incrementInt >> 8) & 0xFF);
byte angleLow =
(byte)(incrementInt & 0xFF);
Serial2.write(0xFE);
Serial2.write(0xFE);
Serial2.write(0x06);
Serial2.write(0x33);
Serial2.write(joint);
Serial2.write(angleHigh);
Serial2.write(angleLow);
Serial2.write(speed);
Serial2.write(0xFA);
}
// =====================================================
// LEFT
// =====================================================
void stepLeft()
{
if (
millis() - lastJ5MoveTime
<
JOINT_INTERVAL_MS
)
{
return;
}
float newPosition =
j5Position + J5_STEP;
if (newPosition > J5_MAX)
{
newPosition = J5_MAX;
}
if (j5Position >= J5_MAX)
{
return;
}
float increment =
newPosition - j5Position;
jogIncrement(
5,
increment,
JOG_SPEED
);
j5Position =
newPosition;
lastJ5MoveTime =
millis();
updateDisplay();
}
// =====================================================
// RIGHT
// =====================================================
void stepRight()
{
if (
millis() - lastJ5MoveTime
<
JOINT_INTERVAL_MS
)
{
return;
}
float newPosition =
j5Position - J5_STEP;
if (newPosition < J5_MIN)
{
newPosition = J5_MIN;
}
if (j5Position <= J5_MIN)
{
return;
}
float increment =
newPosition - j5Position;
jogIncrement(
5,
increment,
JOG_SPEED
);
j5Position =
newPosition;
lastJ5MoveTime =
millis();
updateDisplay();
}
// =====================================================
// UP
// =====================================================
void stepUp()
{
if (
millis() - lastJ4MoveTime
<
JOINT_INTERVAL_MS
)
{
return;
}
float newPosition =
j4Position + J4_STEP;
if (newPosition > J4_MAX)
{
newPosition = J4_MAX;
}
if (j4Position >= J4_MAX)
{
return;
}
float increment =
newPosition - j4Position;
jogIncrement(
4,
increment,
JOG_SPEED
);
j4Position =
newPosition;
lastJ4MoveTime =
millis();
updateDisplay();
}
// =====================================================
// DOWN
// =====================================================
void stepDown()
{
if (
millis() - lastJ4MoveTime
<
JOINT_INTERVAL_MS
)
{
return;
}
float newPosition =
j4Position - J4_STEP;
if (newPosition < J4_MIN)
{
newPosition = J4_MIN;
}
if (j4Position <= J4_MIN)
{
return;
}
float increment =
newPosition - j4Position;
...
This file has been truncated, please download it to see its full contents.
import time
import cv2
import serial
import serial.tools.list_ports
import mediapipe as mp
# =====================================================
# SETTINGS
# =====================================================
CAMERA_INDEX = 0
SERIAL_BAUD = 115200
# =====================================================
# TRACKING ZONES
# =====================================================
STOP_ZONE_X = 45
STOP_ZONE_Y = 35
START_ZONE_X = 70
START_ZONE_Y = 55
# =====================================================
# TIMING
# =====================================================
JOINT_INTERVAL = 0.040
MIN_COMMAND_GAP = 0.018
START_CONFIRM_FRAMES = 2
# Face must really be missing this long
# before NOFACE is generated.
FACE_LOST_DELAY = 1.0
# Stable detections needed after
# HOME / CENTER / genuine NOFACE.
FACE_REACQUIRE_FRAMES = 2
# =====================================================
# SERIAL PORT SELECTION
# =====================================================
ports = list(
serial.tools.list_ports.comports()
)
if not ports:
print("No COM ports found!")
input("Press ENTER to exit...")
raise SystemExit
print()
print("Available COM ports:")
print("--------------------")
for i, port in enumerate(ports):
print(
f"{i}: {port.device} - "
f"{port.description}"
)
print()
try:
choice = int(
input(
"Select M5Stack COM port number: "
)
)
if choice < 0 or choice >= len(ports):
raise ValueError
SERIAL_PORT = ports[choice].device
except Exception:
print("Invalid COM port selection.")
input("Press ENTER to exit...")
raise SystemExit
# =====================================================
# SERIAL CONNECTION
# =====================================================
try:
ser = serial.Serial(
SERIAL_PORT,
SERIAL_BAUD,
timeout=0.02
)
time.sleep(2)
print()
print(
f"Connected to {SERIAL_PORT}"
)
except Exception as e:
print()
print("Serial connection error:")
print(e)
input("Press ENTER to exit...")
raise SystemExit
# =====================================================
# SERIAL SEND
# =====================================================
def send_raw(message):
try:
ser.write(
(message + "\n").encode(
"utf-8"
)
)
ser.flush()
except Exception as e:
print(
"Serial error:",
e
)
def send_command(command):
try:
ser.write(
(command + "\n").encode(
"utf-8"
)
)
print(
"Sent:",
command
)
except Exception as e:
print(
"Serial error:",
e
)
# =====================================================
# STATUS SEND
# =====================================================
last_status_sent = None
def send_status(status):
global last_status_sent
if status == last_status_sent:
return
send_command(status)
last_status_sent = status
# =====================================================
# MEDIAPIPE
# =====================================================
mp_face = mp.solutions.face_detection
face_detector = mp_face.FaceDetection(
model_selection=0,
min_detection_confidence=0.60
)
# =====================================================
# CAMERA
# =====================================================
cap = cv2.VideoCapture(
CAMERA_INDEX
)
if not cap.isOpened():
print("Cannot open camera!")
face_detector.close()
ser.close()
input("Press ENTER to exit...")
raise SystemExit
# =====================================================
# TRACKING STATE
# =====================================================
x_active = False
y_active = False
x_candidate = None
y_candidate = None
x_candidate_count = 0
y_candidate_count = 0
last_x_command = 0.0
last_y_command = 0.0
last_any_command = 0.0
next_axis = "X"
# =====================================================
# WAITING / PAUSE STATE
# =====================================================
waiting_for_face = True
tracking_paused = False
reacquire_count = 0
# =====================================================
# TIME-BASED FACE LOSS
# =====================================================
# None = face is currently detected.
# Otherwise contains the moment at which
# face detection disappeared.
face_missing_since = None
# =====================================================
# RESET AXIS STATES
# =====================================================
def reset_axis_states():
global x_active
global y_active
global x_candidate
global y_candidate
global x_candidate_count
global y_candidate_count
x_active = False
y_active = False
x_candidate = None
y_candidate = None
x_candidate_count = 0
y_candidate_count = 0
# =====================================================
# RESET COMMAND TIMERS
# =====================================================
def reset_command_timers():
global last_x_command
global last_y_command
global last_any_command
global next_axis
now = time.monotonic()
last_x_command = now
last_y_command = now
last_any_command = now
next_axis = "X"
# =====================================================
# READ ARDUINO HANDSHAKE
# =====================================================
def read_arduino_messages():
global tracking_paused
global waiting_for_face
global reacquire_count
global face_missing_since
global last_status_sent
while ser.in_waiting > 0:
try:
line = ser.readline().decode(
"utf-8",
errors="ignore"
).strip()
except Exception:
return
if not line:
continue
# -------------------------------------------------
# HOME / CENTER START
# -------------------------------------------------
if line == "PAUSE_TRACKING":
print()
print(
"ARDUINO -> PAUSE_TRACKING"
)
tracking_paused = True
waiting_for_face = True
reacquire_count = 0
face_missing_since = None
reset_axis_states()
reset_command_timers()
last_status_sent = None
try:
ser.reset_output_buffer()
except Exception:
pass
send_raw(
"PAUSED_ACK"
)
print(
"PYTHON -> PAUSED_ACK"
)
# -------------------------------------------------
# HOME / CENTER FINISHED
# -------------------------------------------------
elif line == "RESUME_WAIT_FACE":
print(
"ARDUINO -> RESUME_WAIT_FACE"
)
tracking_paused = False
waiting_for_face = True
reacquire_count = 0
face_missing_since = None
reset_axis_states()
reset_command_timers()
last_status_sent = None
print(
"STATE -> WAITING_FOR_FACE"
)
print()
# =====================================================
# INFO
# =====================================================
print()
print(
"========================================"
)
print(
" myCobot 2D TRACKING"
)
print(
" TIME-BASED FACE LOSS"
)
print(
"========================================"
)
print()
print(
f"X STOP = +/- {STOP_ZONE_X}"
)
print(
f"X START = +/- {START_ZONE_X}"
)
print(
f"Y STOP = +/- {STOP_ZONE_Y}"
)
print(
f"Y START = +/- {START_ZONE_Y}"
)
print()
print(
f"Joint interval = "
f"{JOINT_INTERVAL * 1000:.0f} ms"
)
print(
f"Minimum gap = "
f"{MIN_COMMAND_GAP * 1000:.0f} ms"
)
print(
f"NO FACE delay = "
f"{FACE_LOST_DELAY:.1f} sec"
)
print()
print(
"BLUE = WAITING / HOME / CENTER"
)
print(
"GREEN = TRACKING"
)
print(
"RED = LOCKED"
)
print(
"YELLOW = REAL FACE LOSS"
)
print()
print(
"ESC = quit"
)
print()
# =====================================================
# MAIN LOOP
# =====================================================
while True:
# =================================================
# READ ARDUINO
# =================================================
read_arduino_messages()
# =================================================
# CAMERA
# =================================================
ok, frame = cap.read()
if not ok:
print("Camera frame error.")
break
height, width = frame.shape[:2]
center_x = width // 2
center_y = height // 2
# =================================================
# CENTER CROSS
# =================================================
cv2.line(
frame,
(center_x, 0),
(center_x, height),
(255, 255, 255),
1
)
cv2.line(
frame,
(0, center_y),
(width, center_y),
(255, 255, 255),
1
)
# =================================================
# GREEN LOCKED ZONE
# =================================================
cv2.rectangle(
frame,
(
center_x - STOP_ZONE_X,
center_y - STOP_ZONE_Y
),
(
center_x + STOP_ZONE_X,
center_y + STOP_ZONE_Y
),
(0, 255, 0),
2
)
# =================================================
# YELLOW START ZONE
# =================================================
cv2.rectangle(
frame,
(
center_x - START_ZONE_X,
center_y - START_ZONE_Y
),
(
center_x + START_ZONE_X,
center_y + START_ZONE_Y
),
(0, 255, 255),
1
)
# =================================================
# FACE DETECTION
# =================================================
rgb = cv2.cvtColor(
frame,
cv2.COLOR_BGR2RGB
)
result = face_detector.process(
rgb
)
face_found = False
camera_status = "WAITING FOR FACE"
# =================================================
# FACE FOUND
# =================================================
if result.detections:
detection = result.detections[0]
confidence = detection.score[0]
bbox = (
detection
.location_data
.relative_bounding_box
)
# ---------------------------------------------
# FACE RECTANGLE
# ---------------------------------------------
x = int(
bbox.xmin * width
)
y = int(
bbox.ymin * height
)
w = int(
bbox.width * width
)
h = int(
bbox.height * height
)
x = max(
0,
x
)
y = max(
0,
y
)
w = min(
w,
width - x
)
h = min(
h,
height - y
)
# ---------------------------------------------
# FACE CENTER
# ---------------------------------------------
face_x = x + w // 2
face_y = y + h // 2
error_x = face_x - center_x
error_y = face_y - center_y
face_found = True
# Any successful detection immediately
# cancels the face-loss timer.
face_missing_since = None
just_reacquired = False
# =================================================
# PAUSED
# =================================================
if tracking_paused:
reacquire_count = 0
camera_status = "PAUSED"
# =================================================
# WAITING FOR FACE
# =================================================
elif waiting_for_face:
reacquire_count += 1
camera_status = "FACE DETECTED"
if (
reacquire_count
>= FACE_REACQUIRE_FRAMES
):
waiting_for_face = False
reacquire_count = 0
reset_axis_states()
reset_command_timers()
# First state after waiting
# is always TRACKING / GREEN.
send_status(
"TRACKING"
)
camera_status = "TRACKING"
just_reacquired = True
# =================================================
# NORMAL TRACKING
# =================================================
if (
not tracking_paused
and
not waiting_for_face
and
not just_reacquired
):
# ---------------------------------------------
# HIGH LEVEL STATUS
# ---------------------------------------------
face_locked = (
abs(error_x) <= STOP_ZONE_X
and
abs(error_y) <= STOP_ZONE_Y
)
if face_locked:
send_status(
"LOCKED"
)
camera_status = "LOCKED"
else:
send_status(
"TRACKING"
)
camera_status = "TRACKING"
# =================================================
# X AXIS
# =================================================
if x_active:
if (
abs(error_x)
<= STOP_ZONE_X
):
x_active = False
x_candidate = None
x_candidate_count = 0
else:
desired_x = None
if (
error_x
> START_ZONE_X
):
desired_x = "RIGHT"
elif (
error_x
< -START_ZONE_X
):
desired_x = "LEFT"
if desired_x is None:
x_candidate = None
x_candidate_count = 0
else:
if (
desired_x
== x_candidate
):
x_candidate_count += 1
else:
x_candidate = desired_x
x_candidate_count = 1
if (
x_candidate_count
>= START_CONFIRM_FRAMES
):
x_active = True
x_candidate = None
x_candidate_count = 0
# =================================================
# Y AXIS
# =================================================
if y_active:
if (
abs(error_y)
<= STOP_ZONE_Y
):
y_active = False
y_candidate = None
y_candidate_count = 0
else:
desired_y = None
if (
error_y
> START_ZONE_Y
):
desired_y = "DOWN"
elif (
error_y
< -START_ZONE_Y
):
desired_y = "UP"
if desired_y is None:
y_candidate = None
y_candidate_count = 0
else:
if (
desired_y
== y_candidate
):
y_candidate_count += 1
else:
y_candidate = desired_y
y_candidate_count = 1
if (
y_candidate_count
>= START_CONFIRM_FRAMES
):
y_active = True
y_candidate = None
y_candidate_count = 0
# =================================================
# CURRENT DIRECTIONS
# =================================================
x_direction = None
y_direction = None
if x_active:
if (
error_x
> STOP_ZONE_X
):
x_direction = "RIGHT"
elif (
error_x
< -STOP_ZONE_X
):
x_direction = "LEFT"
else:
x_active = False
if y_active:
if (
error_y
> STOP_ZONE_Y
):
y_direction = "DOWN"
elif (
error_y
< -STOP_ZONE_Y
):
y_direction = "UP"
else:
y_active = False
# =================================================
# FAST DUAL AXIS SCHEDULER
# =================================================
now = time.monotonic()
global_ready = (
now - last_any_command
>= MIN_COMMAND_GAP
)
if global_ready:
# BOTH AXES
if (
x_direction is not None
and
y_direction is not None
):
if next_axis == "X":
if (
now - last_x_command
>= JOINT_INTERVAL
...
This file has been truncated, please download it to see its full contents.
Comments