Update with encoder values. No calibrate, just reset for now.

ros_as5048b/launch/ros_as5048b.launch.py

@@ -11,6 +11,9 @@ def generate_launch_description():
     i2c_device_arg = DeclareLaunchArgument('i2c_device', default_value="1")
     i2c_address_arg = DeclareLaunchArgument('i2c_address', default_value="0x42")
 
+    encoder_topic_arg = DeclareLaunchArgument('encoder_topic', default_value="/encoder/at_0x42/deg")
+    turn_count_file_arg = DeclareLaunchArgument('turn_count_file', default_value='/tmp/turn_count.yaml')
+
     read_encoder_node = Node(
             package='ros_as5048b',
             executable='read_encoder_node',
@@ -21,27 +24,25 @@ def generate_launch_description():
             ]    
          )
 
-
-    turn_count_file_arg = DeclareLaunchArgument('turn_count_file', default_value='/tmp/turn_count.yaml')
-
     turn_counter_node = Node(
             package='ros_as5048b',
             executable='turn_counter_node',
             name='turn_counter_node',
             parameters=[
                 {'file_path': LaunchConfiguration('turn_count_file')},
-                {'encoder_topic': f"/encoder/at_{LaunchConfiguration('i2c_address')}/deg"},
+                {'encoder_topic': LaunchConfiguration('encoder_topic')},
             ]    
          )
 
     nodes = [
         read_encoder_node,
-        # turn_counter_node
+        turn_counter_node
     ]
 
     return LaunchDescription([
         i2c_device_arg,
         i2c_address_arg,
+        encoder_topic_arg,
         turn_count_file_arg,
     ] + nodes)
 

ros_as5048b/ros_as5048b/turn_counter_node.py

@@ -3,84 +3,66 @@
 
 from std_msgs.msg import Float32
 from std_srvs.srv import Trigger
-from winch_control_interfaces.srv import SetFloat32
 
 import yaml
 import math
 
 class TurnCounterNode(Node):
 
     def __init__(self):
-        super().__init__('tendon_length_node')
+        super().__init__('turn_counter_node')
 
-        self.declare_parameter('file_path', '/tmp/tendon_length.yaml')
+        self.declare_parameter('file_path', '/tmp/turn_count.yaml')
         self.file_path = self.get_parameter('file_path').value
 
         self.declare_parameter('encoder_topic', '/encoder/deg')
         self.encoder_topic = self.get_parameter('encoder_topic').value
 
         self.reset_srv = self.create_service(Trigger, '~/reset', self.reset_callback)
-        self.calibrate_srv = self.create_service(SetFloat32, '~/calibrate', self.calibrate_callback)
 
         self.subscription = self.create_subscription(Float32, self.encoder_topic, self.encoder_callback, 20)
 
-        self.publisher = self.create_publisher(Float32, '~/tendon_length', 5)
+        self.publisher = self.create_publisher(Float32, '~/turn_count', 5)
 
         self.timer = self.create_timer(0.1, self.timer_callback)
         self.timer_counter = 0
 
-        self.tendon_length = self.load_tendon_length_from_file()
+        self.turn_count = self.load_turn_count_from_file()
         self.prev_encoder_value = None  
 
-        self.publish_tendon_length()
-
-
-    def calibrate_callback(self, request, response):
-        self.tendon_length = request.data
-        response.success=True
-        return response
+        self.publish_turn_count()
 
 
     def reset_callback(self, request, response):
-        self.get_logger().info(f"Reset tendon length from {self.tendon_length}")
-        self.tendon_length = 0.0
+        self.get_logger().info(f"Reset turn count from {self.turn_count}")
+        self.turn_count = 0.0
         response.success=True
         return response
 
 
     def timer_callback(self):
-        self.publish_tendon_length()
+        self.publish_turn_count()
 
         self.timer_counter += 1
 
         if self.timer_counter == 10:
-            self.write_tendon_length_to_file()
+            self.write_turn_count_to_file()
             self.timer_counter = 0
 
 
-    def publish_tendon_length(self):
+    def publish_turn_count(self):
         msg_out = Float32()
-        msg_out.data = self.tendon_length
+        msg_out.data = self.turn_count
         self.publisher.publish(msg_out)
 
 
     def encoder_callback(self, encoder_msg):
-
         encoder_value = encoder_msg.data
 
         if self.prev_encoder_value is not None:
-            encoder_movement = self.calculate_movement(encoder_value, self.prev_encoder_value)
-
-            ### TODO: Add more sophisticated tendon length model here
-
-            drum_diameter=0.01
-            drum_circumference=math.pi*drum_diameter
-
-            delta_tendon = drum_circumference * encoder_movement/360.0
-
-            self.tendon_length -= delta_tendon
-
-            ### 
+            encoder_movement = self.calculate_movement(encoder_value, self.prev_encoder_value)      
+            delta_turn = encoder_movement/360.0
+            self.turn_count += delta_turn
 
         self.prev_encoder_value = encoder_value
 
@@ -97,19 +79,19 @@ def calculate_movement(self, current_value, previous_value):
         return movement
 
 
-    def load_tendon_length_from_file(self):
+    def load_turn_count_from_file(self):
         try:
             with open(self.file_path, 'r') as file:
                 data = yaml.safe_load(file)
-                if 'tendon_length' in data:
-                    return data['tendon_length']
+                if 'turn_count' in data:
+                    return data['turn_count']
         except (FileNotFoundError, yaml.YAMLError, TypeError):
             pass
         return 0.0  # Default value if file doesn't exist or is invalid
 
 
-    def write_tendon_length_to_file(self):
-        data = {'tendon_length': self.tendon_length}
+    def write_turn_count_to_file(self):
+        data = {'turn_count': self.turn_count}
         with open(self.file_path, 'w') as file:
             yaml.dump(data, file)