import cv2
import numpy as np
import matplotlib.pyplot as plt
# PID controller constants
Kp = 0.1
Ki = 0.0
Kd = 0.1
# Initial error values
last_error = 0
integral = 0
# Create an empty image
img = np.zeros((512, 512, 3), np.uint8)
# Target angle (90 degrees upward)
target_angle = -np.pi/2
# Create line using OpenCV
line_length = 200
line_angle = np.pi/2 # Initial heading angle
line_color = (0, 0, 255) # Red
line_thickness = 3
line_start_point_x = round(512/2)
line_start_point_y = line_start_point_x
line_start_point = (line_start_point_x, line_start_point_y)
# Flag to indicate if PID controller is active
pid_active = True
x_last_pos = None
# Initialize lists to store angle and target values
angle_list = []
target_list = []
error_list = []
# Mouse callback function to update line angle
def update_line_angle(event, x, y, flags, param):
global line_angle, pid_active, x_last_pos
if event == cv2.EVENT_MBUTTONDOWN:
pid_active = False
x_last_pos = x
elif event == cv2.EVENT_MOUSEMOVE and flags == cv2.EVENT_FLAG_MBUTTON:
delta = x - x_last_pos
x_last_pos = x
line_angle += delta*0.05
elif event == cv2.EVENT_MBUTTONUP:
pid_active = True
# Set mouse callback function
cv2.namedWindow('frame')
cv2.setMouseCallback('frame', update_line_angle)
while True:
img = np.zeros((512, 512, 3), np.uint8)
# Calculate line end point based on angle and length
line_end_point = (
int(line_start_point[0] - line_length * np.cos(line_angle)),
int(line_start_point[1] - line_length * np.sin(line_angle))
)
# Draw line on frame
cv2.line(img, line_start_point, line_end_point, line_color, line_thickness)
# Display frame
cv2.imshow('frame', img)
# Calculate current line angle relative to target angle
current_angle = np.arctan2(line_end_point[1] - line_start_point[1],
line_end_point[0] - line_start_point[0])
error = target_angle - current_angle
# Update PID controller values
if pid_active:
integral += error
derivative = error - last_error
last_error = error
# Calculate PID output
output = Kp * error + Ki * integral + Kd * derivative
# Update line angle based on PID output
line_angle += output
# Store angle and target values
angle_list.append(current_angle)
target_list.append(target_angle)
error_list.append(error)
# Exit program if 'q' key is pressed
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# Plot angle and target values over time
fig, axs = plt.subplots(2)
axs[0].plot(angle_list, label='Angle')
axs[0].plot(target_list, label='Target')
axs[1].plot(error_list, label='Error')
plt.legend()
plt.show()
# Destroy windows
cv2.destroyAllWindows()
This code basically draws rotating line which has no physical proporties about a axis, and PID control over that line, also you can change angle of it by dragging with middle mouse button while pressed. My target angle is 90 degree upward.
The problem is when I set angle to between 180 and 90 degree, controller taking clockwise direction, but otherwise it takes anticlockwise which means when I cross 180 degree a bit it takes long angle. I am not able to fix it. I am pretty sure there are things about opencv coordinate system. I am so confused rigth now.