Hello, everyone.
I am working on a project where a camera tracks a ball and my program controls the position of a beam attached stepper motor on which the ball rolls to keep the ball in the middle of the beam. I am implementing a PID controller.
Details:
Using Python 3.11 - OpenCV for a red ball's detection
FHD Webcam camera position above the ball and beam stepup, with a clear view of everything needed.
Stepper Motor Nema 17 (17PM-K406-11V), 1.4A current rating, Uni-Polar
A4988 Stepper Motor Driver, powered from a 12V battery pack, Half-step enabled.
Step pin connected to pin 3 on Uno. Direction on pin 2.
Arduino Uno
I am using Python's OpenCV library to track the ball on the beam. A red coloured ball's distance from the centre of the camera frame is being calculated and stored in 'distance' variable. This effectively is the error term. This error term is manipulated to control the position of the stepper motor. A control signal generated using the error term is sent to my Arduino Uno using the Pyserial library using a Baud Rate of 115200.
The issue I am having is that the stepper motor is very slow to spin. I believe that this is because of the stepper.run() function I am using from the AccelStepper library, which moves the motor one step at a time, far to slow for my application. I have failed to find an alternative function that accelerates the motor to the required angular position. I tried using stepper.runToPosition() but it blocks the code and makes the motor spin fast round and round, to and fro, which I cannot understand.
Here is my Python code:
import numpy as np
import cv2
import serial
#Defining PID gains
kp = 0.5
kd = 0
ki =0
last_error =0
stepper_pos=0
integral=0
ser = serial.Serial('com11', 115200)
def measure_distance(frame, center):
"""Measures the distance between the center of the frame and the red object.
Args:
frame: A numpy array representing the frame.
center: A tuple (x, y) representing the center of the frame.
Returns:
A float representing the distance between the center of the frame and the red object.
"""
# Convert the frame to HSV color space.
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# Define the HSV range for red color.
redLower1 = (0, 100, 100)
redUpper1 = (10, 255, 255)
redLower2 = (160, 100, 100)
redUpper2 = (180, 255, 255)
# Create masks for both red color ranges.
mask1 = cv2.inRange(hsv, redLower1, redUpper1)
mask2 = cv2.inRange(hsv, redLower2, redUpper2)
mask = cv2.bitwise_or(mask1, mask2)
# Erode and dilate the mask to remove noise.
mask = cv2.erode(mask, None, iterations=2)
mask = cv2.dilate(mask, None, iterations=2)
# Find the contours of the red object.
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
# Find the center of the red object.
if len(cnts) > 0:
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
# Calculate the distance between the center of the frame and the red object.
distance = np.linalg.norm(np.array([x, y]) - center)
distance = round(distance, 0)
# Draw a circle around the red object.
cv2.circle(frame, (int(x), int(y)), int(radius), (0, 0, 255), 2)
cv2.circle(frame, (int(x), int(y)), 4, (0, 255, 255), 1)
if y > center[1]: #uncomment if using
distance *= -1
# Print the distance to the console.
# print("Distance to red object:", distance)
if (distance > 100):
distance=100
elif distance <-100:
distance = -100
return distance
else:
return None
# Capture the frame from the camera.
cap = cv2.VideoCapture(0)
# Get the center of the frame.
center = (cap.get(cv2.CAP_PROP_FRAME_WIDTH) // 2, cap.get(cv2.CAP_PROP_FRAME_HEIGHT) // 2)
while True:
# Read the next frame from the camera.
ret, frame = cap.read()
# If the frame is not empty, measure the distance of the red object from the center of the frame.
if ret:
distance = measure_distance(frame, center)
if (distance!=None):
#start of PID code
error = distance
integral+=error
control_signal = (kp * error) - kd*(error - last_error) + (ki*integral) #PID code (integral to be added)
#updating the lastdistance
last_error = error
# stepper_pos = stepper_pos + program_output
stepper_mover = str(control_signal) + "\r" #type casting
#writing the position_output to the motor controller
ser.write(stepper_mover.encode())
ser.flush()
print(stepper_mover) #for testing
# If the distance is not None, print it to the screen and draw a circle around the red object.
if distance is not None:
# Print the distance to the screen.
cv2.putText(frame, "Distance to red object: {}".format(distance), (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
center = (int(center[0]), int(center[1]))
# Draw a circle around the red object.
cv2.circle(frame, center, 5, (0, 255, 0), 2)
# Display the frame.
# cv2.resize("Frame", frame,200,300)
cv2.imshow("Frame", frame)
# If the 'q' key is pressed, break the loop.
if cv2.waitKey(1) & 0xFF == ord("q"):
break
# Release the camera and close all windows.
cap.release()
cv2.destroyAllWindows()
Here is the code I am running on the Arduino Uno.
#include <AccelStepper.h>
const int step_pin = 3;
const int direction_pin = 2;
int required_pos;
int checker_variable;
// Define a stepper and the pins it will use
AccelStepper stepper(1,step_pin, direction_pin);
void setup()
{
//note: I have used half-microstepping, so any input we receive we must multiply by 2
stepper.setMaxSpeed(3000);
stepper.setAcceleration(10000);
stepper.setSpeed(6000);
// stepper.moveTo(800);
Serial.begin(115200);
// stepper.moveTo(200);
}
void loop()
{
// Read new position
if (stepper.distanceToGo()==0){
Serial.print("Enter a position to go to: ");
Serial.flush();
// if (Serial.available()!=0){
checker_variable= Serial.parseInt();
if (checker_variable!=0){ //to prevent the Serial Monitor newline from being read, resetting the motor
required_pos=checker_variable;
}
Serial.println(required_pos);}
stepper.moveTo(required_pos);
stepper.run();
}
Any advice on how to make the stepper motor snappy and responsive to the position of the ball would be greatly appreciated. I have seen many such projects but almost no project does what I'm doing with a camera. Usually, an ultrasonic sensor is used.
Best,
Shameer.
