chore: restructured project

Experiments/PidTest/Scripts/pid_test.gd

@@ -0,0 +1,115 @@
+extends Node3D
+
+var p: float = 0.4
+var i: float = 0.0
+var d: float = 1
+
+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_node = get_node("Spinner")
+	target_node = get_node("Target")
+
+	p_input = get_node("PID Menue/PanelContainer/VBoxContainer/Proportinal Term (P)/Input")
+	p_input.value_changed.connect(_update_p_value)
+
+	i_input = get_node("PID Menue/PanelContainer/VBoxContainer/Integral (I)/Input")
+	i_input.value_changed.connect(_update_i_value)
+
+	d_input = get_node("PID Menue/PanelContainer/VBoxContainer/Derivative (D)/Input")
+	d_input.value_changed.connect(_update_d_value)
+
+	force_input = get_node("PID Menue/PanelContainer/VBoxContainer/Force/Input")
+	force_input.value_changed.connect(_update_force_value)
+
+	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 _update_p_value(value: float) -> void:
+	p = value
+
+
+func _update_i_value(value: float) -> void:
+	i = value
+
+
+func _update_d_value(value: float) -> void:
+	d = value
+
+
+func _update_force_value(value: float) -> void:
+	force = 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) + " %"