Dec-28-2024, 12:10 AM
(This post was last modified: Dec-28-2024, 10:40 PM by classified.
Edit Reason: solved the issue
)
Hello,
I'm working on a robotic head and I have a pan/tilt mechanism with a 2 axis tilt (Left/Right tilt).
I'm using a camera with python to do motion tracking. I got that working, but when I terminate the program, my servo's go to their home positions (as they should), but after termination the servos will randomly move at random times (For example, the head will pan from it's home position (90deg) to 180deg, then return to 90deg). This happens with any servo at any given time when the program is no longer running.
(If I upload/run just my Arduino code, I don't get any random movements. It's only once I run the python program, then terminate it. Hence, the reason why I believe it's a python issue)
It's driving me crazy, and I can't figure out why it's doing that or how to stop it.
Any help/insight on this issue would be greatly appreciated.
Thanks in advance.
(Also, I've had an earlier version of my code where it was just the pan servo and I didn't have this issue. I have included that below in case that helps. I am stumped why my new code seems to have this issue)
Python Code:
I'm working on a robotic head and I have a pan/tilt mechanism with a 2 axis tilt (Left/Right tilt).
I'm using a camera with python to do motion tracking. I got that working, but when I terminate the program, my servo's go to their home positions (as they should), but after termination the servos will randomly move at random times (For example, the head will pan from it's home position (90deg) to 180deg, then return to 90deg). This happens with any servo at any given time when the program is no longer running.
(If I upload/run just my Arduino code, I don't get any random movements. It's only once I run the python program, then terminate it. Hence, the reason why I believe it's a python issue)
It's driving me crazy, and I can't figure out why it's doing that or how to stop it.
Any help/insight on this issue would be greatly appreciated.
Thanks in advance.
(Also, I've had an earlier version of my code where it was just the pan servo and I didn't have this issue. I have included that below in case that helps. I am stumped why my new code seems to have this issue)
Python Code:
#-------------------------------------------------------
# Motion Tracking TEST 2
#-------------------------------------------------------
# Working Motion Tracking (Pan/Tilt)
# Has 10sec no motion timeout
# Servo returns to home pos when program closed
# ISSUE: When program terminated servos move randomly
#-------------------------------------------------------
# pip install mediapipe
# pip install --upgrade mediapipe protobuf
#-------------------------------------------------------
#-------------------------------------------------------
# Arduino Code:
#-------------------------------------------------------
# #include <Servo.h>
# Servo panServo;
# Servo tiltRightServo;
# Servo tiltLeftServo;
# void setup() {
# panServo.attach(9); // Replace with your actual pins
# tiltRightServo.attach(10); //(R)
# tiltLeftServo.attach(11); //(L)
# panServo.write(90); // Set to neutral position (adjust as needed)
# tiltRightServo.write(55); // Home position for left tilt (adjust as needed)
# tiltLeftServo.write(70); // Home position for right tilt (adjust as needed) up<90<down
# delay(500); // Allow time for servos to stabilize
# Serial.begin(9600); // Initialize serial communication
# }
# void loop() {
# // Normal operation (e.g., receiving commands via Serial)
# if (Serial.available() >= 3) {
# int panPos = Serial.read();
# int tiltLeftPos = Serial.read();
# int tiltRightPos = Serial.read();
# if (panPos == 255 && tiltLeftPos == 255 && tiltRightPos == 255) {
# panServo.detach();
# tiltRightServo.detach();
# tiltLeftServo.detach();
# } else {
# if (!panServo.attached()) panServo.attach(9);
# if (!tiltRightServo.attached()) tiltRightServo.attach(10);
# if (!tiltLeftServo.attached()) tiltLeftServo.attach(11);
# panServo.write(panPos);
# tiltRightServo.write(tiltLeftPos);
# tiltLeftServo.write(tiltRightPos);
# }
# }
# }
#-------------------------------------------------------
import cv2
import mediapipe as mp
import serial
import time
import sys
# Set up serial connection to Arduino (adjust COM port as needed)
arduino = serial.Serial('COM9', 9600)
time.sleep(2) # Allow time for the connection to establish
# Initialize MediaPipe Pose
mp_pose = mp.solutions.pose
pose = mp_pose.Pose()
# Set up webcam
cap = cv2.VideoCapture(0)
# Frame dimensions
frame_width = 640
frame_height = 480
# Define center of the frame
center_x = frame_width / 2
center_y = frame_height / 2 # Vertical center for tilt
# Define servo position limits
SERVO_LEFT = 0
SERVO_CENTER = 90
SERVO_RIGHT = 180
# Servo limits for tilt (same as pan, but can be adjusted)
TILT_LEFT = 30 #0
TILT_RIGHT = 140 #180
# Home positions for the servos (change these to match your setup)
home_pan_position = 90 # Neutral position for pan
home_left_tilt_position = 55 # Home position for left tilt
home_right_tilt_position = 85 # Home position for right tilt
# Variables to track the last known servo positions
last_pan_position = home_pan_position
last_tilt_left_position = home_left_tilt_position
last_tilt_right_position = home_right_tilt_position
# Define movement thresholds
MOVEMENT_THRESHOLD = 70 # Minimum pixel change to trigger servo movement
PAN_INCREMENT = 5 # How much to move the pan servo on each update
TILT_INCREMENT = 5 # How much to move the tilt servos on each update
# Dead zone for centering
DEAD_ZONE = 50 # Define a dead zone around the center
# Timer variables
last_motion_time = time.time()
MOTION_TIMEOUT = 10 # Timeout duration in seconds
# Smooth movement function
def smooth_move_servo(current_position, target_position, increment, min_pos, max_pos):
"""
Smoothly move from current_position to target_position.
Increments the position towards the target and limits to min/max bounds.
"""
if current_position < target_position:
current_position = min(current_position + increment, target_position)
elif current_position > target_position:
current_position = max(current_position - increment, target_position)
# Ensure the position stays within servo bounds
current_position = max(min_pos, min(max_pos, current_position))
return current_position
try:
while cap.isOpened():
ret, frame = cap.read()
if not ret:
break
# Flip the frame horizontally
frame = cv2.flip(frame, 1)
rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
results = pose.process(rgb_frame)
motion_detected = False # Flag to track if motion is detected
if results.pose_landmarks:
# Get the x and y coordinates of the nose
nose_x = results.pose_landmarks.landmark[mp_pose.PoseLandmark.NOSE].x * frame_width
nose_y = results.pose_landmarks.landmark[mp_pose.PoseLandmark.NOSE].y * frame_height
# Calculate the differences from the center
center_difference_x = nose_x - center_x
center_difference_y = nose_y - center_y
# Determine the new pan (left-right) servo position
if abs(center_difference_x) > MOVEMENT_THRESHOLD:
motion_detected = True # Motion detected for pan
if center_difference_x < -DEAD_ZONE: # Nose is left of center and outside dead zone
new_pan_position = max(last_pan_position - PAN_INCREMENT, SERVO_LEFT)
elif center_difference_x > DEAD_ZONE: # Nose is right of center and outside dead zone
new_pan_position = min(last_pan_position + PAN_INCREMENT, SERVO_RIGHT)
else:
new_pan_position = last_pan_position # Stay in the current position if within the dead zone
else:
new_pan_position = last_pan_position # No movement detected for pan
# Smoothly move the pan servo
last_pan_position = smooth_move_servo(last_pan_position, new_pan_position, PAN_INCREMENT, SERVO_LEFT, SERVO_RIGHT)
# Determine the new tilt (up/down) servo positions based on nose_y (vertical position)
if abs(center_difference_y) > MOVEMENT_THRESHOLD:
motion_detected = True # Motion detected for tilt
if center_difference_y < -DEAD_ZONE: # Nose is above center (head up)
new_tilt_left_position = min(last_tilt_left_position + TILT_INCREMENT, TILT_RIGHT)
new_tilt_right_position = max(last_tilt_right_position - TILT_INCREMENT, TILT_LEFT)
elif center_difference_y > DEAD_ZONE: # Nose is below center (head down)
new_tilt_left_position = max(last_tilt_left_position - TILT_INCREMENT, TILT_LEFT)
new_tilt_right_position = min(last_tilt_right_position + TILT_INCREMENT, TILT_RIGHT)
else:
new_tilt_left_position = last_tilt_left_position # Stay in the current position if within the dead zone
new_tilt_right_position = last_tilt_right_position # Stay in the current position
else:
new_tilt_left_position = last_tilt_left_position # No movement detected for tilt
new_tilt_right_position = last_tilt_right_position # No movement detected for tilt
# Smoothly move the tilt servos
last_tilt_left_position = smooth_move_servo(last_tilt_left_position, new_tilt_left_position, TILT_INCREMENT, TILT_LEFT, TILT_RIGHT)
last_tilt_right_position = smooth_move_servo(last_tilt_right_position, new_tilt_right_position, TILT_INCREMENT, TILT_LEFT, TILT_RIGHT)
# Send the servo positions to the Arduino
arduino.write(bytes([last_pan_position, last_tilt_left_position, last_tilt_right_position]))
# Update last nose positions for future reference
last_nose_x = nose_x
last_nose_y = nose_y
# Reset the timer if motion is detected
if motion_detected:
last_motion_time = time.time()
# Check if the timeout period has been exceeded
if time.time() - last_motion_time > MOTION_TIMEOUT:
# Reset all servos to the home positions after timeout
last_pan_position = home_pan_position # Neutral position for pan
last_tilt_left_position = home_left_tilt_position # Neutral position for left tilt
last_tilt_right_position = home_right_tilt_position # Neutral position for right tilt
print(f"No motion detected for {MOTION_TIMEOUT} seconds. Moving all servos to home positions.")
arduino.write(bytes([last_pan_position, last_tilt_left_position, last_tilt_right_position]))
# Draw landmarks on the frame
mp.solutions.drawing_utils.draw_landmarks(frame, results.pose_landmarks, mp_pose.POSE_CONNECTIONS)
cv2.imshow('Motion Tracking', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
except KeyboardInterrupt:
pass # Handle graceful exit if interrupted
finally:
# Ensure no further communication is sent to the Arduino
print(f"Exiting... Moving servos to home positions: Pan={home_pan_position}, Left Tilt={home_left_tilt_position}, Right Tilt={home_right_tilt_position}")
# Send neutral positions to stop all movement
arduino.write(bytes([home_pan_position, home_left_tilt_position, home_right_tilt_position])) # Neutral positions for all servos
time.sleep(1) # Wait a moment for the servos to stop moving
# Close the serial port to ensure no further communication
print("Closing serial connection...")
arduino.close()
cap.release()
cv2.destroyAllWindows()Older Pan Code (For Reference)#-------------------------------------------------------
# Motion Tracking
#-------------------------------------------------------
# Working Motion Tracking (Pan Only)
# Has 10sec no motion timeout
# Servo returns to 90 when program closed
#-------------------------------------------------------
# pip install mediapipe
# pip install --upgrade mediapipe protobuf
#-------------------------------------------------------
#-------------------------------------------------------
# Arduino Code:
#-------------------------------------------------------
# #include <Servo.h> // Include the Servo library
# Servo myServo; // Create a servo object to control a servo
# int servoPin = 9; // Define the pin connected to the servo (adjust if necessary)
# void setup() {
# Serial.begin(9600); // Initialize serial communication at 9600 baud
# myServo.attach(servoPin); // Attach the servo to the specified pin
# myServo.write(90); // Initialize the servo to the center position (90 degrees)
# Serial.println("Servo Ready"); // Optional: Debug message
# }
# void loop() {
# // Check if serial data is available
# if (Serial.available() > 0) {
# int angle = Serial.read(); // Read the incoming byte as an angle value
# // Check if the received value is within the valid range for a servo (0 to 180)
# if (angle >= 0 && angle <= 180) {
# myServo.write(angle); // Move the servo to the received angle
# Serial.print("Servo moved to: ");
# Serial.println(angle); // Optional: Debug message
# } else {
# Serial.println("Invalid angle received"); // Optional: Debug message for out-of-range values
# }
# }
# }
#-------------------------------------------------------
#-------------------------------------------------------
import cv2
import mediapipe as mp
import serial
import time
import sys
# Set up serial connection to Arduino
arduino = serial.Serial('COM9', 9600)
time.sleep(2) # Allow time for connection to establish
# Initialize MediaPipe Pose
mp_pose = mp.solutions.pose
pose = mp_pose.Pose()
# Set up webcam
cap = cv2.VideoCapture(0)
# Frame dimensions
frame_width = 640
frame_height = 480
# Define center of the frame
center_x = frame_width / 2
# Define servo position limits
SERVO_LEFT = 0
SERVO_CENTER = 90
SERVO_RIGHT = 180
# Variables to track the last known servo position and last nose position
last_servo_position = SERVO_CENTER
last_nose_x = center_x
# Define movement thresholds
MOVEMENT_THRESHOLD = 70 # Minimum pixel change to trigger servo movement
PAN_INCREMENT = 5 # How much to move the servo on each update
# Dead zone for centering
DEAD_ZONE = 50 # Define a dead zone around the center
# Timer variables
last_motion_time = time.time()
MOTION_TIMEOUT = 10 # Timeout duration in seconds
try:
while cap.isOpened():
ret, frame = cap.read()
if not ret:
break
# Flip the frame horizontally
frame = cv2.flip(frame, 1)
rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
results = pose.process(rgb_frame)
motion_detected = False # Flag to track if motion is detected
if results.pose_landmarks:
# Get the x coordinate of the nose
nose_x = results.pose_landmarks.landmark[mp_pose.PoseLandmark.NOSE].x * frame_width
#print(f"Nose Position X: {nose_x}")
# Calculate the difference from the center
center_difference = nose_x - center_x
# Determine the new servo position
if abs(center_difference) > MOVEMENT_THRESHOLD:
motion_detected = True # Motion detected
if center_difference < -DEAD_ZONE: # Nose is left of center and outside dead zone
new_servo_position = max(last_servo_position - PAN_INCREMENT, SERVO_LEFT)
elif center_difference > DEAD_ZONE: # Nose is right of center and outside dead zone
new_servo_position = min(last_servo_position + PAN_INCREMENT, SERVO_RIGHT)
else:
new_servo_position = last_servo_position # Stay in the current position if within the dead zone
# Only update if the new position is different
if new_servo_position != last_servo_position:
print(f"Moving Servo to: {new_servo_position}")
last_servo_position = new_servo_position
arduino.write(bytes([last_servo_position]))
else:
new_servo_position = last_servo_position # Maintain current position if no movement
# Update last nose position
last_nose_x = nose_x
# Reset the timer if motion is detected
if motion_detected:
last_motion_time = time.time()
# Check if the timeout period has been exceeded
if time.time() - last_motion_time > MOTION_TIMEOUT:
last_servo_position = SERVO_CENTER # Reset to center position
print(f"No motion detected for {MOTION_TIMEOUT} seconds. Moving Servo to: {SERVO_CENTER}")
arduino.write(bytes([last_servo_position]))
# Draw landmarks on the frame
mp.solutions.drawing_utils.draw_landmarks(frame, results.pose_landmarks, mp_pose.POSE_CONNECTIONS)
cv2.imshow('Motion Tracking', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
except KeyboardInterrupt:
pass # Handle graceful exit if interrupted
finally:
# Move the servo to the default position before closing
print(f"Exiting... Moving Servo to: {SERVO_CENTER}")
arduino.write(bytes([SERVO_CENTER]))
time.sleep(1) # Wait a moment for the servo to move
cap.release()
cv2.destroyAllWindows()
arduino.close()
