haylan/Godot-Experements
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: added pid calculation and better ui

Browse Source
This commit is contained in:
Arthur Erlich
2025-12-23 00:29:18 +01:00
parent f41b760208
commit 45ccd0bedb
2 changed files with 251 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
Levels/pid_test.gd
View File

@@ -1,14 +1,90 @@
extends Node3D
@export var p:float = 0.5
@export var i:float = 0.0
@export var d:float = 0.0
@export var force:float = 10.0
var p:float = 0.4
var i:float = 0.0
var d:float = 1
var spinner:Node3D
var target:Node3D
var force:float = 200.0
var error:float = 0.0
var last_error = 0.0
var error_derivative:float = 0.0
var integral:float = 0.0
var target:float = 0.0 #0.0 -> 359.99
var p_input:SpinBox
var i_input:SpinBox
var d_input:SpinBox
var force_input:SpinBox
var target_input:SpinBox
var spinner_node:Node3D
var target_node:Node3D
var error_lable:Label
var speed_lable:Label
var acceleration_label:Label
var error_derivative_lable:Label
var integral_lable:Label
var spinner_speed:float = 0.0
var max_speed:float = 120 # deg/m -> 1/360/60s = 1rpm
var acceleration:float
func _ready() -> void:
spinner = get_node("Spinner")
target = get_node("Target")
spinner_node = get_node("Spinner")
target_node = get_node("Target")
p_input = get_node("PID Menue/PanelContainer/VBoxContainer/Proportinal Term (P)/Input")
i_input = get_node("PID Menue/PanelContainer/VBoxContainer/Integral (I)/Input")
d_input = get_node("PID Menue/PanelContainer/VBoxContainer/Derivative (D)/Input")
force_input = get_node("PID Menue/PanelContainer/VBoxContainer/Force/Input")
target_input =get_node("PID Menue/PanelContainer/VBoxContainer/Target Rotation/Input")
target_input.value_changed.connect(_update_target_rot)
error_lable = get_node("PID Menue/PanelContainer/VBoxContainer/Error/Output")
speed_lable = get_node("PID Menue/PanelContainer/VBoxContainer/Speed/Output")
error_derivative_lable = get_node("PID Menue/PanelContainer/VBoxContainer/Error Derivative/Output")
acceleration_label = get_node("PID Menue/PanelContainer/VBoxContainer/Acceleration/Output")
integral_lable = get_node("PID Menue/PanelContainer/VBoxContainer/Integral/Output")
# Set default values
p_input.value = p
i_input.value = i
d_input.value = d
force_input.value = force
func _update_target_rot(value:float) -> void:
target_node.rotation.x = deg_to_rad(value)
func _process(delta: float) -> void:
# PID Stuff
target = rad_to_deg(target_node.rotation.x)
error = target - rad_to_deg(spinner_node.rotation.x)
error_derivative = (error - last_error) /delta
integral += error * delta
# Clamp acceleration between min and max force
var pid_output := (p * error) + (i * integral) + (d * error_derivative)
acceleration = clampf(pid_output, -force, force)
# Set speed
var next_speed = spinner_speed + acceleration * delta
# Block acceleration that would exceed max speed
if abs(spinner_speed) >= max_speed and sign(acceleration) == sign(spinner_speed):
acceleration = 0.0
next_speed = spinner_speed
spinner_speed = clamp(next_speed, -max_speed, max_speed)
spinner_node.rotation_degrees.x += spinner_speed * delta
last_error = error
func _physics_process(delta: float) -> void:
#UI Updats
error_lable.text = str(error).pad_decimals(2).pad_zeros(3) + " deg"
error_derivative_lable.text = str(error_derivative).pad_decimals(2).pad_zeros(3)
integral_lable.text = str(integral).pad_decimals(2).pad_zeros(3)
speed_lable.text = str(spinner_speed).pad_decimals(2).pad_zeros(3) + " deg/m"
acceleration_label.text = str(acceleration/force*100).pad_decimals(2).pad_zeros(3) + " %"