diff --git a/Levels/pid_test.gd b/Levels/pid_test.gd index 3f6c071..1fbaa9e 100644 --- a/Levels/pid_test.gd +++ b/Levels/pid_test.gd @@ -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) + " %" diff --git a/Levels/pid_test.tscn b/Levels/pid_test.tscn index 7300b79..502e024 100644 --- a/Levels/pid_test.tscn +++ b/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_bottom = 40.0 +offset_left = 20.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