2024 Auto (#27)

* time-based auto

* After first run

* After second run

launch/auto.launch.py

@@ -1,7 +1,40 @@
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from ament_index_python import get_package_share_directory
+import os.path
 
 def generate_launch_description():
-    return LaunchDescription([
+    #Auto control node
+    start_auto_node = Node(
+        package='control_pkg',
+        executable='auto',
+        name='auto'
+    )
+
+    #Axle manager for Hyflex node
+    start_axle_manager_node = Node(
+        package='axle_manager',
+        executable='axle_manager',
+        name='axle_manager',
+        arguments=['--params-file '+os.path.join(get_package_share_directory('axle_manager'),'config','params.yaml')]
+    )
 
-    ])
+    #VN100 Sensor IMU Node
+    start_imu_node = Node(
+        package='sensors_pkg',
+        executable='imu',
+        name='imu'
+    )
+
+
+
+    #Declare launch description and populate
+    ld = LaunchDescription()
+
+    #declare launch actions
+    ld.add_action(start_axle_manager_node)
+    ld.add_action(start_auto_node)
+
+    return ld

launch/fqr.launch.py

@@ -0,0 +1,40 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from ament_index_python import get_package_share_directory
+import os.path
+
+def generate_launch_description():
+    #Auto control node
+    start_fqr_node = Node(
+        package='control_pkg',
+        executable='fqr',
+        name='fqr'
+    )
+
+    #Axle manager for Hyflex node
+    start_axle_manager_node = Node(
+        package='axle_manager',
+        executable='axle_manager',
+        name='axle_manager',
+        arguments=['--params-file '+os.path.join(get_package_share_directory('axle_manager'),'config','params.yaml')]
+    )
+
+    #VN100 Sensor IMU Node
+    start_imu_node = Node(
+        package='sensors_pkg',
+        executable='imu',
+        name='imu'
+    )
+
+
+
+    #Declare launch description and populate
+    ld = LaunchDescription()
+
+    #declare launch actions
+    ld.add_action(start_axle_manager_node)
+    ld.add_action(start_fqr_node)
+
+    return ld

src/axle_manager/axle_manager/hdc2460_node.py

@@ -21,8 +21,8 @@ def __init__(self):
                 ('leftChannel',1),
                 ('rightChannel',2),
                 ('maxSpeed',500),
-                ('accelRate',30000),
-                ('brakeRate',10000),
+                ('accelRate',31860),
+                ('brakeRate',10620),
                 ('pivotDevice',"FAC"),
                 ('pivotExtendChannel',1),
                 ('pivotRetractChannel',2),

src/control_pkg/control_pkg/auto.py

@@ -0,0 +1,227 @@
+import rclpy
+
+from rclpy.node import Node
+from std_msgs.msg import Int8, Float32
+from geometry_msgs.msg import Twist
+from utilities.tools import Tools
+import time
+
+class Auto(Node):
+
+    def __init__(self):
+        super().__init__('auto')
+
+        self.running = False
+        self.state = 0
+        self.pivot_updated = False
+        self.pivot_position = 0
+        self.degree_deadband = 0.2
+
+        self.speed_publisher = self.create_publisher(Twist, '/cmd_vel', 10)
+        self.pivot_publisher = self.create_publisher(Int8, '/vehicle/pivot', 10)
+
+        self.pivot_sub = self.create_subscription(Float32, '/sensor/pivot', self.update_pivot, 10)
+
+        self.create_timer(0.01, self.determine_control)
+
+    def determine_control(self):
+        if not self.pivot_updated:
+            return
+        match self.state:
+            case 0:
+                self.get_logger().info('turning to 0 degrees')
+                self.turn_to_degrees(0)
+                return
+            case 1:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 2:
+                self.get_logger().info('driving at 100% speed for 1 meter')
+                self.drive_distance(1.0, 2.16)
+                return
+            case 3:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 4:
+                self.get_logger().info('turning to -20.75 degrees')
+                self.turn_to_degrees(-20.75)
+                return
+            case 5:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 6:
+                self.get_logger().info('driving at 100% speed for 3.14 meters')
+                self.drive_distance(1.0, 3.14)
+                return
+            case 7:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 8:
+                self.get_logger().info('turning to 0 degrees')
+                self.turn_to_degrees(0)
+                return
+            case 9:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 10:
+                self.get_logger().info('driving at 100% speed for 1.5 meters')
+                self.drive_distance(1.0, 2.5)
+                return
+            case 11:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 12:
+                self.get_logger().info('driving at -100% speed for 1.5 meters')
+                self.drive_distance(-1.0, 2.25)
+                return
+            case 13:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 14:
+                self.get_logger().info('turning to -20.75 degrees')
+                self.turn_to_degrees(-20.75)
+                return
+            case 15:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 16:
+                self.get_logger().info('driving at -100% speed for 3.14 meters')
+                self.drive_distance(-1.0, 3.3)
+                return
+            case 17:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 18:
+                self.get_logger().info('turning to 20.75 degrees')
+                self.turn_to_degrees(20.75)
+                return
+            case 19:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 20:
+                self.get_logger().info('driving at 100% speed for 3.14 meters')
+                self.drive_distance(1.0, 3.4)
+                return
+            case 21:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 22:
+                self.get_logger().info('turing to 0 degrees')
+                self.turn_to_degrees(0)
+                return
+            case 23:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 24:
+                self.get_logger().info('driving at 100% speed for 1.5 meters')
+                self.drive_distance(1.0, 2.5)
+                return
+            case 25:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 26:
+                self.get_logger().info('driving at -100% speed for 1.5 meters')
+                self.drive_distance(-1.0, 2.5)
+                return
+            case 27:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 28:
+                self.get_logger().info('turning to 20.75 degrees')
+                self.turn_to_degrees(20.75)
+                return
+            case 29:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 30:
+                self.get_logger().info('driving at -100% speed for 3.14 meters')
+                self.drive_distance(-1.0, 3.4)
+                return
+            case 31:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 32:
+                self.get_logger().info('turning to 0 degrees')
+                self.turn_to_degrees(0)
+                return
+            case 33:
+                self.get_logger().info('waiting for 2 seconds')
+                self.wait_time(2)
+                return
+            case 34:
+                self.get_logger().info('driving at -100% speed for 1 meter')
+                self.drive_distance(-1.0, 2.16)
+                return
+            case _:
+                self.get_logger().info('done')
+                return
+
+    def turn_to_degrees(self, degrees):
+        msg = Int8()
+        if abs(self.pivot_position - degrees) <= self.degree_deadband:
+            self.state += 1
+            msg.data = 0
+        else:
+            msg.data = 1 if self.pivot_position < degrees else -1
+        self.pivot_publisher.publish(msg)
+
+    def drive_distance(self, speed, distance):
+        run_time = 0
+        if distance >= 2/3:
+            run_time = (1/3) + distance - (2/3)
+        else:
+            run_time = ((distance / (2/3)) ** 2) / 3
+        self.drive_speed_time(speed, run_time)
+
+    def drive_speed_time(self, speed, run_time):
+        if not self.running:
+            self.end_time = time.time() + run_time
+            self.running = True
+        msg = Twist()
+        if time.time() >= self.end_time:
+            msg.linear.x = 0.0
+            self.state += 1
+            self.running = False
+        else:
+            msg.linear.x = speed
+        self.speed_publisher.publish(msg)
+
+    def wait_time(self, run_time):
+        if not self.running:
+            self.end_time = time.time() + run_time
+            self.running = True
+        if time.time() >= self.end_time:
+            self.state += 1
+            self.running = False
+
+    def update_pivot(self, msg: Float32):
+        self.pivot_updated = True
+        self.pivot_position = Tools.potentiometer_to_degrees(msg.data)
+
+def main():
+    rclpy.init()
+
+    node = Auto()
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/control_pkg/control_pkg/fqr.py

@@ -0,0 +1,30 @@
+import rclpy
+
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+
+class FQR(Node):
+
+    def __init__(self):
+        super().__init__('fqr')
+
+        self.speed_publisher = self.create_publisher(Twist, '/cmd_vel', 10)
+
+        self.create_timer(.01, self.publish_speed)
+
+    def publish_speed(self):
+        msg = Twist()
+        msg.linear.x = 1.0
+        self.speed_publisher.publish(msg)
+
+def main():
+    rclpy.init()
+
+    node = FQR()
+    rclpy.spin(node)
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()