From 3c8f0912046722f86459d77445b0a3c7f7c93e16 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=D0=92=D0=B8=D0=BE=D0=BB=D0=B5=D1=82=D1=82=D0=B0=20=D0=90?=
 =?UTF-8?q?=D0=B9=D0=B2=D0=B0=D0=B7=D1=8C=D1=8F=D0=BD?=
 <violet.aiwa@icloud.com>
Date: Sat, 30 Nov 2024 14:20:32 +0000
Subject: [PATCH 1/2] Add pid regulstor

---
 lesson_06/step_1.py | 71 +++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 71 insertions(+)
 create mode 100644 lesson_06/step_1.py

diff --git a/lesson_06/step_1.py b/lesson_06/step_1.py
new file mode 100644
index 0000000..03340ec
--- /dev/null
+++ b/lesson_06/step_1.py
@@ -0,0 +1,71 @@
+import math
+from math import degrees
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import PoseStamped
+from std_msgs.msg import Float64
+from tf_transformations import euler_from_quaternion
+
+
+def angle_diff(a1, a2):
+    a = a1 - a2
+    return (a + math.pi) % (2 * math.pi) - math.pi
+
+
+def get_yaw_from_quaternion(q):
+    return euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
+
+
+class RurMover(Node):
+    def __init__(self):
+        super().__init__('controller')
+
+        # Создаем публикаторы и подписчики
+        self.publisher_state = self.create_publisher(Float64, '/waterstrider/pid/state', 10)
+        self.publisher_setpoint = self.create_publisher(Float64, '/waterstrider/pid/setpoint', 10)
+        self.result = self.create_subscription(Float64, '/waterstrider/pid/output', self.result_callback, 10)
+
+        self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10)
+        self.pose_subscriber = self.create_subscription(PoseStamped,'/waterstrider/ground_truth_to_tf_waterstrider/pose',self.pose_callback, 10)
+
+        # Переменные состояния
+        self.heading = None
+        self.desired_heading = -math.pi / 2  # Целевой угол
+        self.LINEAR_SPEED = 1.0
+        self.SIDE_TIME = 10.0
+        self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback)
+
+    def pose_callback(self, msg):
+        self.heading = get_yaw_from_quaternion(msg.pose.orientation)  # Текущий угол робота
+        mes_heading = Float64()
+        mes_heading.data = self.heading  # Установка текущего угла в сообщение
+        self.publisher_state.publish(mes_heading)  # Публикация текущего угла
+        
+    def pub_timer_callback(self):
+        self.desired_heading += math.pi / 2  # Увеличиваем целевой угол на 90 градусов
+        self.desired_heading %= 2 * math.pi  # [0, 2π]
+        message = Float64()
+        message.data = self.desired_heading
+        self.publisher_setpoint.publish(message)  # Публикуем новый целевой угол
+
+    def result_callback(self, msg):
+        msg_pub = Twist()
+        msg_pub.linear.x = self.LINEAR_SPEED
+        msg_pub.angular.z = msg.data
+        self.publisher_twist.publish(msg_pub)  # Публикуем команду движения робота
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    mover = RurMover()
+
+    rclpy.spin(mover)
+
+    mover.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
-- 
GitLab


From a345f044233dc833af76e7f7a617e316b946ecb8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=D0=92=D0=B8=D0=BE=D0=BB=D0=B5=D1=82=D1=82=D0=B0=20=D0=90?=
 =?UTF-8?q?=D0=B9=D0=B2=D0=B0=D0=B7=D1=8C=D1=8F=D0=BD?=
 <violet.aiwa@icloud.com>
Date: Sat, 30 Nov 2024 14:22:40 +0000
Subject: [PATCH 2/2] Coefficients changed

---
 config/control-config.yaml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/config/control-config.yaml b/config/control-config.yaml
index 80bbe66..e4fe347 100644
--- a/config/control-config.yaml
+++ b/config/control-config.yaml
@@ -4,8 +4,8 @@
         angular: true
         clamp: 15.0
         inverted: false
-        kd: 0.9
+        kd: 1.0
         ki: 0.0
-        kp: 0.04
+        kp: 8.0
         start_type_description_service: true
         use_sim_time: false
-- 
GitLab