feat: added pid calculation and better ui

Levels/pid_test.gd

@@ -1,14 +1,90 @@
 extends Node3D
 
-@export var p:float = 0.5
+var p:float = 0.4
-@export var i:float = 0.0
+var i:float = 0.0
-@export var d:float = 0.0
+var d:float = 1
-@export var force:float = 10.0
 
-var spinner:Node3D
+var force:float = 200.0
-var target:Node3D
+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")
+	spinner_node = get_node("Spinner")
-	target = get_node("Target")
+	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) + " %"

Levels/pid_test.tscn

@@ -1,4 +1,4 @@
-[gd_scene load_steps=5 format=3 uid="uid://caj5t8q1fii5m"]
+[gd_scene load_steps=8 format=3 uid="uid://caj5t8q1fii5m"]
 
 [ext_resource type="PackedScene" uid="uid://cin3pgpeydkpe" path="res://Assets/PID_Spinning_Target.glb" id="1_0c8sw"]
 [ext_resource type="Script" uid="uid://b63aqs33qyp0g" path="res://Levels/pid_test.gd" id="1_sxbsa"]
@@ -9,6 +9,13 @@ background_mode = 2
 background_color = Color(0.14478481, 0.14478481, 0.14478481, 1)
 ambient_light_source = 2
 
+[sub_resource type="Theme" id="Theme_sxbsa"]
+
+[sub_resource type="Theme" id="Theme_d03hh"]
+
+[sub_resource type="LabelSettings" id="LabelSettings_sxbsa"]
+font_size = 24
+
 [node name="PidTest" type="Node3D"]
 script = ExtResource("1_sxbsa")
 
@@ -36,29 +43,179 @@ transform = Transform3D(0.25000006, 0.9659258, 0.06698735, -0.2588191, -1.908172
 [node name="PID Menue" type="Control" parent="."]
 layout_mode = 3
 anchors_preset = 0
-offset_right = 40.0
+offset_left = 20.0
-offset_bottom = 40.0
+offset_top = 20.0
+offset_right = 20.0
+offset_bottom = 20.0
+theme = SubResource("Theme_sxbsa")
 
-[node name="VBoxContainer" type="VBoxContainer" parent="PID Menue"]
+[node name="PanelContainer" type="PanelContainer" parent="PID Menue"]
 layout_mode = 0
 offset_right = 40.0
 offset_bottom = 40.0
 
-[node name="HBoxContainer" type="HBoxContainer" parent="PID Menue/VBoxContainer"]
+[node name="VBoxContainer" type="VBoxContainer" parent="PID Menue/PanelContainer"]
+layout_mode = 2
+theme = SubResource("Theme_d03hh")
+
+[node name="Spinner Header" type="Label" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+text = "Spinner Controll"
+label_settings = SubResource("LabelSettings_sxbsa")
+
+[node name="Proportinal Term (P)" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
 layout_mode = 2
 
-[node name="Proportinal Term (P)" type="Label" parent="PID Menue/VBoxContainer/HBoxContainer"]
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Proportinal Term (P)"]
 layout_mode = 2
+size_flags_horizontal = 3
 text = "Proportinal Term (P)"
 
-[node name="Proportinal Term (P) Input" type="SpinBox" parent="PID Menue/VBoxContainer/HBoxContainer"]
+[node name="MarginContainer" type="MarginContainer" parent="PID Menue/PanelContainer/VBoxContainer/Proportinal Term (P)"]
+custom_minimum_size = Vector2(20, 0)
 layout_mode = 2
 
-[node name="HBoxContainer2" type="HBoxContainer" parent="PID Menue/VBoxContainer"]
+[node name="Input" type="SpinBox" parent="PID Menue/PanelContainer/VBoxContainer/Proportinal Term (P)"]
+layout_mode = 2
+max_value = 10.0
+step = 0.01
+value = 0.5
+
+[node name="Integral (I)" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
 layout_mode = 2
 
-[node name="Label" type="Label" parent="PID Menue/VBoxContainer/HBoxContainer2"]
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Integral (I)"]
+layout_mode = 2
+size_flags_horizontal = 3
+text = "Integral (I)"
+
+[node name="Input" type="SpinBox" parent="PID Menue/PanelContainer/VBoxContainer/Integral (I)"]
+layout_mode = 2
+max_value = 10.0
+step = 0.01
+
+[node name="Derivative (D)" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
 layout_mode = 2
 
-[node name="LineEdit" type="SpinBox" parent="PID Menue/VBoxContainer/HBoxContainer2"]
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Derivative (D)"]
 layout_mode = 2
+size_flags_horizontal = 3
+text = "Derivative (D)"
+
+[node name="Input" type="SpinBox" parent="PID Menue/PanelContainer/VBoxContainer/Derivative (D)"]
+layout_mode = 2
+max_value = 10.0
+step = 0.01
+
+[node name="Force" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Force"]
+layout_mode = 2
+size_flags_horizontal = 3
+text = "Force"
+
+[node name="Input" type="SpinBox" parent="PID Menue/PanelContainer/VBoxContainer/Force"]
+layout_mode = 2
+
+[node name="MarginContainer" type="MarginContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+custom_minimum_size = Vector2(0, 20)
+layout_mode = 2
+
+[node name="Target Header" type="Label" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+text = "Target"
+label_settings = SubResource("LabelSettings_sxbsa")
+
+[node name="Target Rotation" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Target Rotation"]
+layout_mode = 2
+size_flags_horizontal = 3
+text = "Target Rotation"
+
+[node name="Input" type="SpinBox" parent="PID Menue/PanelContainer/VBoxContainer/Target Rotation"]
+layout_mode = 2
+max_value = 1439.96
+step = 0.01
+custom_arrow_step = 10.0
+
+[node name="MarginContainer2" type="MarginContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+custom_minimum_size = Vector2(0, 20)
+layout_mode = 2
+
+[node name="Solution" type="Label" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+text = "Solution"
+label_settings = SubResource("LabelSettings_sxbsa")
+
+[node name="Error" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Error"]
+layout_mode = 2
+size_flags_horizontal = 3
+text = "Error"
+
+[node name="Output" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Error"]
+layout_mode = 2
+text = "000.00"
+horizontal_alignment = 1
+uppercase = true
+
+[node name="Error Derivative" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Error Derivative"]
+layout_mode = 2
+size_flags_horizontal = 3
+text = "Error Derivative"
+
+[node name="Output" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Error Derivative"]
+layout_mode = 2
+text = "000.00"
+horizontal_alignment = 1
+uppercase = true
+
+[node name="Integral" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Integral"]
+layout_mode = 2
+size_flags_horizontal = 3
+text = "Integral"
+
+[node name="Output" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Integral"]
+layout_mode = 2
+text = "000.00"
+horizontal_alignment = 1
+uppercase = true
+
+[node name="Speed" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Speed"]
+layout_mode = 2
+size_flags_horizontal = 3
+text = "Speed"
+
+[node name="Output" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Speed"]
+layout_mode = 2
+text = "000.00"
+horizontal_alignment = 1
+uppercase = true
+
+[node name="Acceleration" type="HBoxContainer" parent="PID Menue/PanelContainer/VBoxContainer"]
+layout_mode = 2
+
+[node name="Label" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Acceleration"]
+layout_mode = 2
+size_flags_horizontal = 3
+text = "Acceleration"
+
+[node name="Output" type="Label" parent="PID Menue/PanelContainer/VBoxContainer/Acceleration"]
+layout_mode = 2
+text = "000.00"
+horizontal_alignment = 1
+uppercase = true