diff --git a/config.yaml b/config.yaml index c8d4161..f606670 100644 --- a/config.yaml +++ b/config.yaml @@ -6,8 +6,19 @@ lite6_y_limit_upper: 0.1475 lite6_z_lift_amount: 0.01 - lite6_z_offset: 0.1475 + lite6_z_offset: 0.201942 lite6_blend_radius: 0.0 lite6_max_velocity_scaling_factor: 1.0 lite6_max_acceleration_scaling_factor: 0.9 + + axidraw_accel: 100 + axidraw_const_speed: false + axidraw_pen_delay_down: 0 + axidraw_pen_delay_up: 0 + axidraw_pen_pos_down: 40 + axidraw_pen_pos_up: 60 + axidraw_pen_rate_lower: 50 + axidraw_pen_rate_raise: 75 + axidraw_speed_pendown: 100 + axidraw_speed_penup: 100 diff --git a/src/axidraw_controller/config/config.yaml b/src/axidraw_controller/config/config.yaml index e69de29..ae3c039 100644 --- a/src/axidraw_controller/config/config.yaml +++ b/src/axidraw_controller/config/config.yaml @@ -0,0 +1,12 @@ +/axidraw_serial: + ros__parameters: + axidraw_accel: 100 + axidraw_const_speed: false + axidraw_pen_delay_down: 0 + axidraw_pen_delay_up: 0 + axidraw_pen_pos_down: 40 + axidraw_pen_pos_up: 60 + axidraw_pen_rate_lower: 50 + axidraw_pen_rate_raise: 75 + axidraw_speed_pendown: 100 + axidraw_speed_penup: 100 diff --git a/src/axidraw_controller/launch/axidraw_controller.launch.py b/src/axidraw_controller/launch/axidraw_controller.launch.py index dd0910d..6439235 100644 --- a/src/axidraw_controller/launch/axidraw_controller.launch.py +++ b/src/axidraw_controller/launch/axidraw_controller.launch.py @@ -1,4 +1,5 @@ import os +import yaml from launch import LaunchDescription from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -18,6 +19,13 @@ def launch_setup(context, *args, **kwargs): serial_port = LaunchConfiguration('serial_port', default='/dev/ttyACM0') log_level = LaunchConfiguration("log_level", default='info') + config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('axidraw_controller'), 'config', 'config.yaml'])) + + robotcontroller_config = config.perform(context) + with open(robotcontroller_config, 'r') as f: + axidraw_config = yaml.safe_load(f)['/axidraw_serial']['ros__parameters'] + print(f'Loaded configuration: {axidraw_config}') + nodes = [ Node( package="axidraw_controller", @@ -34,6 +42,7 @@ def launch_setup(context, *args, **kwargs): arguments=["--ros-args", "--log-level", log_level], parameters=[ {"serial_port": serial_port}, + axidraw_config, ], ), ] diff --git a/src/axidraw_controller/src/py/axidraw_serial.py b/src/axidraw_controller/src/py/axidraw_serial.py index 0985b14..cad1ab1 100755 --- a/src/axidraw_controller/src/py/axidraw_serial.py +++ b/src/axidraw_controller/src/py/axidraw_serial.py @@ -61,13 +61,18 @@ class AxidrawSerial(Node): return False self.ad.options.units = 2 # set working units to mm. self.ad.options.model = 2 # set model to AxiDraw V3/A3 - self.ad.options.speed_pendown = 100 # 100% speed - self.ad.options.speed_penup = 100 # 100% speed - self.ad.options.accel = 100 # 100% speed - self.ad.options.pen_rate_lower = 100 # 100% speed - self.ad.options.pen_rate_raise = 100 # 100% speed - #self.ad.options.pen_delay_down = 50 - #self.ad.options.pen_delay_up = 50 + + self.ad.options.speed_pendown = self.get_parameter('axidraw_speed_pendown').get_parameter_value().integer_value # Maximum XY speed when the pen is down (plotting). + self.ad.options.speed_penup = self.get_parameter('axidraw_speed_penup').get_parameter_value().integer_value # Maximum XY speed when the pen is up. + self.ad.options.accel = self.get_parameter('axidraw_accel').get_parameter_value().integer_value # Relative acceleration/deceleration speed. + self.get_logger().error('accel:{}'.format(self.get_parameter('axidraw_accel').get_parameter_value().integer_value)) + self.ad.options.pen_pos_down = self.get_parameter('axidraw_pen_pos_down').get_parameter_value().integer_value #Pen height when the pen is down (plotting). + self.ad.options.pen_pos_up = self.get_parameter('axidraw_pen_pos_up').get_parameter_value().integer_value #Pen height when the pen is up. + self.ad.options.pen_rate_lower = self.get_parameter('axidraw_pen_rate_lower').get_parameter_value().integer_value # Speed of lowering the pen-lift motor. + self.ad.options.pen_rate_raise = self.get_parameter('axidraw_pen_rate_raise').get_parameter_value().integer_value #Speed of raising the pen-lift motor. + self.ad.options.const_speed = self.get_parameter('axidraw_const_speed').get_parameter_value().bool_value #Option: Use constant speed when pen is down. + self.ad.options.pen_delay_down = self.get_parameter('axidraw_pen_delay_down').get_parameter_value().integer_value #Added delay after lowering pen. + self.ad.options.pen_delay_up = self.get_parameter('axidraw_pen_delay_up').get_parameter_value().integer_value #Added delay after raising pen. self.ad.update() # Process changes to options self.status["serial"] = "ready" self.status["motion"] = "ready" @@ -89,6 +94,17 @@ class AxidrawSerial(Node): self.declare_parameter('serial_port', '/dev/ttyACM0') port = self.get_parameter('serial_port').get_parameter_value().string_value + self.declare_parameter('axidraw_speed_pendown', 100) + self.declare_parameter('axidraw_speed_penup', 100) + self.declare_parameter('axidraw_accel', 100) + self.declare_parameter('axidraw_pen_pos_down', 40) + self.declare_parameter('axidraw_pen_pos_up', 60) + self.declare_parameter('axidraw_pen_rate_lower', 50) + self.declare_parameter('axidraw_pen_rate_raise', 75) + self.declare_parameter('axidraw_const_speed', False) + self.declare_parameter('axidraw_pen_delay_down', 0) + self.declare_parameter('axidraw_pen_delay_up', 0) + self.status_srv = self.create_service(Status, 'axidraw_status', self.get_status) while not self.init_serial(port):