why this happens when I cross 180 degree?

107 Views Asked by At
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.

0

There are 0 best solutions below