Ball-Chasing PID Controller Demo

pid.py

import tkinter as tk
import random

class Application(tk.Frame):
    kp = 0.01
    ki = 0.0
    kd = 0.0
    prev_error_x = 0.0
    prev_error_y = 0.0
    sum_error_x = 0.0
    sum_error_y = 0.0
    timestep = 20
    vector_lines = []


    def __init__(self, master=None):
        super().__init__(master)
        self.master = master
        self.grid()
        self.create_widgets()
        self.start_debugging()


    def create_widgets(self):
        # Create the canvas
        self.canvas = tk.Canvas(self.master, width=400, height=400)
        self.canvas.grid(columnspan=3)
        
        # Create the ball
        self.ball = Ball(self.canvas, 200, 200, 10)

        # Create the triangle
        self.triangle = Triangle(self.canvas, random.randint(50, 350), random.randint(50, 350), 10)

        # Create the box
        self.box = Box(self.canvas, 10, 10, 390, 390)
        
        # Create the speed slider
        self.speed_slider = tk.Scale(self.master, from_=0, to=10, length=600, resolution=0.1,
                                orient='horizontal', command=self.update_speed, label="Ball Speed")
        self.speed_slider.grid(columnspan=3)
        
        # Create the ball direction slider
        self.dir_slider = tk.Scale(self.master, from_=-5, to=5, length=600,
                                orient='horizontal', command=self.update_direction, label="Ball Direction")
        self.dir_slider.grid(columnspan=3)

        # Create PID controller sliders for the triangle
        self.kp_slider = tk.Scale(self.master, from_=0, to=1, resolution=0.01, length=600,
                                  orient='horizontal', label='kp', command=self.update_pid)
        self.kp_slider.grid(columnspan=3)

        self.ki_slider = tk.Scale(self.master, from_=0, to=0.1, resolution=0.0001, length=600,
                                  orient='horizontal', label='ki', command=self.update_pid)
        self.ki_slider.grid(columnspan=3)

        self.kd_slider = tk.Scale(self.master, from_=0, to=.02, resolution=0.0001, length=600,
                                  orient='horizontal', label='kd', command=self.update_pid)
        self.kd_slider.grid(columnspan=3)

        # Create the reset buttons
        self.reset_button = tk.Button(self.master, text="Reset All", command=self.reset_all)
        self.reset_button.grid(columnspan=3)

        # Start the simulation
        self.animate()

    def animate(self):
        # Animate the ball
        self.ball.move()
        
        # Bounce off walls
        x1, y1, x2, y2 = self.canvas.coords(self.ball.id)
        if x1 <= 10 or x2 >= 390:
            self.ball.dx *= -1
        if y1 <= 10 or y2 >= 390:
            self.ball.dy *= -1


        # Move the triangle towards the ball using PID controller
        x_ball, y_ball = self.ball.get_center()
        x_triangle, y_triangle = self.triangle.get_center()
        error_x = x_ball - x_triangle
        error_y = y_ball - y_triangle

        # Calculate P, I, D terms
        p_term_x = self.kp * error_x
        i_term_x = self.ki * self.sum_error_x
        d_term_x = self.kd * (error_x - self.prev_error_x) / (self.timestep / 1000)
        p_term_y = self.kp * error_y
        i_term_y = self.ki * self.sum_error_y
        d_term_y = self.kd * (error_y - self.prev_error_y) / (self.timestep / 1000)

        control_x = p_term_x + i_term_x + d_term_x
        control_y = p_term_y + i_term_y + d_term_y

        # Draw P, I, D term vectors
        # first clear previous vector lines
        for line in self.vector_lines:
            self.canvas.delete(line)
        self.vector_lines = []
        self.vector_lines.append(self.draw_vector(x_triangle, y_triangle, p_term_x, p_term_y, 'red'))
        self.vector_lines.append(self.draw_vector(x_triangle, y_triangle, i_term_x, i_term_y, 'green'))
        self.vector_lines.append(self.draw_vector(x_triangle, y_triangle, d_term_x, d_term_y, 'blue'))

        self.triangle.move(control_x, control_y)

        self.prev_error_x = error_x
        self.prev_error_y = error_y
        self.sum_error_x += error_x * self.timestep / 1000
        self.sum_error_y += error_y * self.timestep / 1000

        self.after(self.timestep, self.animate)

    def update_plot(self, plot, canvas, value):
        # Add new value to the plot
        plot.plot([value], [0], marker='o', markersize=3, color='b')
        canvas.draw()

    def reset_all(self):
        self.sum_error_x = 0
        self.sum_error_y = 0
        self.prev_error_x = 0
        self.prev_error_y = 0
        # change the triangle pos to random spot on the canvas
        self.triangle.moveto(random.randint(50, 350), random.randint(50, 350))

    def draw_vector(self, x_start, y_start, dx, dy, color):
        scale_factor = 20  # Adjust as necessary to make vectors visible
        x_end = x_start + dx * scale_factor
        y_end = y_start + dy * scale_factor
        return self.canvas.create_line(x_start, y_start, x_end, y_end, fill=color, width=2)


    def start_debugging(self):
        # Print all diagnostic's of the simulation
        # print(f"kp: {self.kp}, ki: {self.ki}, kd: {self.kd}")
        # print(f"prev_error_x: {self.prev_error_x}, prev_error_y: {self.prev_error_y}")
        # print(f"sum_error_x: {self.sum_error_x}, sum_error_y: {self.sum_error_y}")
        self.after(500, self.start_debugging)
        
    def update_speed(self, speed):
        # Update the ball speed
        self.ball.set_speed(float(speed))

    def update_direction(self, direction):
        # Update the ball direction
        x = direction
        y = 5 - abs(float(x)) #idk
        self.ball.set_direction(float(x), float(y))
    
    def update_pid(self, _=None):
        self.kp = self.kp_slider.get()
        self.ki = self.ki_slider.get()
        self.kd = self.kd_slider.get()

class Ball:
    def __init__(self, canvas, x, y, radius):
        self.canvas = canvas
        self.id = canvas.create_oval(x-radius, y-radius, x+radius, y+radius, fill='red')
        self.dx = 1
        self.dy = 1
    
    def move(self):
        # Move the ball
        self.canvas.move(self.id, self.dx, self.dy)
        
    def set_speed(self, speed):
        # Set the speed
        magnitude = ((self.dx**2 + self.dy**2)**0.5)
        self.dx = (self.dx / magnitude) * speed
        self.dy = (self.dy / magnitude) * speed
    
    def set_direction(self, x, y):
        # Set the direction
        self.dx = x
        self.dy = y
    
    def get_center(self):
        # Get the coordinates
        points = self.canvas.coords(self.id)

        return (points[0] + points[2]) / 2, (points[1] + points[3]) / 2
    
class Triangle:
    def __init__(self, canvas, x, y, size):
        self.canvas = canvas
        self.id = canvas.create_polygon(x, y, x+size, y, x+size/2, y+size, fill='blue')

    def move(self, dx, dy):
        self.canvas.move(self.id, dx, dy)

    def moveto(self, x, y):
        self.canvas.coords(self.id, x, y, x+10, y, x+5, y+10)

    def get_center(self):
        points = self.canvas.coords(self.id)
        center_x = sum(points[::2]) / 3
        center_y = sum(points[1::2]) / 3
        return center_x, center_y

class Box:
    def __init__(self, canvas, x1, y1, x2, y2):
        self.canvas = canvas
        self.id = canvas.create_rectangle(x1, y1, x2, y2)

root = tk.Tk()
app = Application(master=root)
app.mainloop()