Control fix

main_pkg/CMakeLists.txt

@@ -32,7 +32,7 @@ install(TARGETS
 ament_python_install_package(${PROJECT_NAME})
 # Install Python executables
 install(PROGRAMS
-  scripts/py_node.py
+  scripts/control_node.py
   scripts/fake_gps.py
   scripts/real_gps.py
   scripts/web.py

main_pkg/external_scripts/feetback.py

@@ -0,0 +1,19 @@
+# Initialize the variables
+alpha = 0.2  # Smoothing factor
+filtered_value = 10
+import time
+
+while True:
+    # Read the input value
+    input_value = 0
+
+    # If it's the first iteration, initialize the filtered value
+    if filtered_value is None:
+        filtered_value = input_value
+    else:
+        # Apply the exponential smoothing formula
+        filtered_value = alpha * input_value + (1 - alpha) * filtered_value
+
+    # Use the filtered value for further processing
+    print(filtered_value)
+    time.sleep(0.1)

main_pkg/launch/0camera.launch.py

@@ -4,9 +4,7 @@
 from launch_ros.actions import Node
 
 def generate_launch_description():
-
-
-
+
     return LaunchDescription([
         Node(
             package='web_video_server',

main_pkg/launch/2motor.launch.py

@@ -5,12 +5,10 @@
 
 def generate_launch_description():
 
-
-
     return LaunchDescription([
         Node(
             package='main_pkg',
-            executable='py_node.py',
-            name='my_node'
+            executable='control_node.py',
+            name='control_node'
         )
     ])

main_pkg/launch/3web_control.launch.py

@@ -33,7 +33,7 @@ def generate_launch_description():
         # )
         Node(
             package='main_pkg',
-            executable='py_node.py',
-            name='my_node'
+            executable='control_node.py',
+            name='control_node'
         )
     ])

main_pkg/scripts/py_node.py → main_pkg/scripts/control_node.py

@@ -5,19 +5,16 @@
 import RPi.GPIO as GPIO
 import math
 import time
-import asyncio
 
-TIMEOUT = 0.5
+TIMEOUT = 1
 class CmdVelSubscriber(Node):
 
     def __init__(self):
         super().__init__('cmd_vel_subscriber')
-        self.subscription = self.create_subscription(
-            Twist,
-            'cmd_vel',
-            self.listener_callback,
-            10)
-        self.subscription
+        self.subscription = self.create_subscription(Twist,'cmd_vel',self.listener_callback,10)
+
+        timer_period = 0.5  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
 
         GPIO.setmode(GPIO.BCM)
         GPIO.setup(18, GPIO.OUT)
@@ -33,50 +30,57 @@ def __init__(self):
         self.pwm13.start(0)
         self.pwm19.start(0)
         self.linear_vel = 0
+        self.linear_msg = 0
+
         self.angular_vel = 0
+        self.angular_msg = 0
 
+        # low pass parameter
+        self.alpha = 0.2
+        self.beta = 35
+        self.filtered_value = 10
+        self.l_distance = 1
         self.last_msg_time = time.time()
-        # self.create_task(self.check_timeout())
 
     def listener_callback(self, msg):
+        self.linear_msg = msg.linear.x
+        self.angular_msg = msg.angular.z
+        self.last_msg_time = time.time()
+
+    def timer_callback(self):
+
+        if time.time() - self.last_msg_time > TIMEOUT:
+            self.linear_msg = 0
+            self.angular_msg = 0       
 
-        # We use the linear_x-linear_vel to slowi the changes of the speed
-        # The constant 0.1 is represent how fast the changes is
-        self.linear_vel  = (msg.linear.x - self.linear_vel)*0.1
-        self.angular_vel = -(msg.angular.z - self.angular_vel)*0.1
-        l_distance = 1
+        # We use the low pass filter to slow the changes of the speed
+        # The constant alpha is represent how fast the changes is
+        self.linear_vel = self.alpha*self.linear_msg + (1 - self.alpha)*self.linear_vel
+        self.angular_vel = self.alpha*self.angular_msg + (1 - self.alpha)*self.angular_vel
 
-        wheel_left  = self.linear_vel - self.angular_vel*l_distance/2
-        wheel_right = self.linear_vel + self.angular_vel*l_distance/2
+        wheel_left  = self.linear_vel - self.angular_vel*self.l_distance/2
+        wheel_right = self.linear_vel + self.angular_vel*self.l_distance/2
+
+        # print("wheel_left: {}".format(wheel_left))
+        # print("wheel_right: {}".format(wheel_right))
 
         # Use the velocities to control the PWM signal
         # We use the first order transer function to convert from 0->inf to 0->1:
-        # y=(1-e^(-|x|/2))*100
+        # y=(1-e^(-|x|*35))*100
         if wheel_left < 0:
-            self.pwm18.ChangeDutyCycle((1-math.exp(-abs(wheel_left)/2))*100)
+            self.pwm18.ChangeDutyCycle((1-math.exp(-abs(wheel_left)*self.beta))*100)
             self.pwm12.ChangeDutyCycle(0)
         else:
             self.pwm18.ChangeDutyCycle(0)
-            self.pwm12.ChangeDutyCycle((1-math.exp(-abs(wheel_left)/2))*100)
+            self.pwm12.ChangeDutyCycle((1-math.exp(-abs(wheel_left)*self.beta))*100)
 
         if wheel_right < 0:
             self.pwm19.ChangeDutyCycle(0)
-            self.pwm13.ChangeDutyCycle((1-math.exp(-abs(wheel_right)/2))*60)
+            self.pwm13.ChangeDutyCycle((1-math.exp(-abs(wheel_right)*self.beta))*100)
         else:
-            self.pwm19.ChangeDutyCycle((1-math.exp(-abs(wheel_right)/2))*60)
+            self.pwm19.ChangeDutyCycle((1-math.exp(-abs(wheel_right)*self.beta))*100)
             self.pwm13.ChangeDutyCycle(0)
 
-        self.last_msg_time = time.time()
-
-    # async def check_timeout(self):
-    #     while rclpy.ok():
-    #         if time.time() - self.last_msg_time > TIMEOUT:
-    #             self.pwm18.ChangeDutyCycle(0)
-    #             self.pwm12.ChangeDutyCycle(0)
-    #             self.pwm13.ChangeDutyCycle(0)
-    #             self.pwm19.ChangeDutyCycle(0)
-    #         await asyncio.sleep(1)
-
 def main(args=None):
     rclpy.init(args=args)
 
@@ -89,4 +93,4 @@ def main(args=None):
         cmd_vel_subscriber.destroy_node()
 
 if __name__ == '__main__':
-    main()
+    main()