diff --git a/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py b/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
index 05e75e71b508ab6e18fba33595758d2bd511ffe8..6c760539ef459b5f233f4d4665230368f73a5a3c 100644
--- a/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
+++ b/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
@@ -31,6 +31,9 @@ def generate_launch_description():
     velocity_constant_launch_arg = DeclareLaunchArgument('velocity_constant', default_value = '0.015')
     velocity_constant = LaunchConfiguration('velocity_constant')
 
+    constant_speed_launch_arg = DeclareLaunchArgument('constant_speed', default_value = 'True') # change
+    constant_speed = LaunchConfiguration('constant_speed')
+
 
     return LaunchDescription([
         velocity1_launch_arg,
@@ -38,6 +41,7 @@ def generate_launch_description():
         slope_launch_arg,
         velocity3_launch_arg,
         velocity_constant_launch_arg,
+        constant_speed_launch_arg,
         Node(
             package = package_name,
             executable = 'forward_kinematics_node_1dof',
@@ -55,7 +59,8 @@ def generate_launch_description():
                 {'velocity2': velocity2},
                 {'velocity3': velocity3},
                 {'velocity_constant': velocity_constant},
-                {'slope': slope}
+                {'slope': slope},
+                {'constant_speed': constant_speed}
             ]
         ),
         Node(
diff --git a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
index 2456a00c2ef55e89cf060c5d0d96823d0e558801..5e1de2e8279ff9f8728e0cf0a5851360f1cb616b 100644
--- a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
+++ b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
@@ -18,20 +18,20 @@ class Reference_Node_1DoF(Node):
         self.declare_parameter('velocity3', 0.0)
         self.declare_parameter('velocity_constant', 0.0)
         self.declare_parameter('slope', 0.0)
+        self.declare_parameter('constant_speed', False)
 
         self.velocity1 = self.get_parameter('velocity1').get_parameter_value().double_value
         self.velocity2 = self.get_parameter('velocity2').get_parameter_value().double_value
         self.velocity3 = self.get_parameter('velocity3').get_parameter_value().double_value
         self.velocity_constant = self.get_parameter('velocity_constant').get_parameter_value().double_value
         self.slope = self.get_parameter('slope').get_parameter_value().double_value
+        self.constant_speed = self.get_parameter('constant_speed').get_parameter_value().bool_value
 
         self.phi_start = 0.0
         self.phi_end = 1.0
         self.phi = self.phi_start
         self.delta = self.velocity_constant
 
-        self.constant_speed = False
-
         self.timer_period = 0.1  # seconds
         self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
         # List the points
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
index 0c3482d0a7e304824a9dd062de9dca7cde61ce93..88d3ad94a952c258e32fa7419e3676f7deb3b2b5 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
@@ -64,18 +64,6 @@ class Reference_Node_2DoF(Node):
                 else:
                     self.phi = self.phi + self.velocity2
 
-
-
-        # if self.constant_speed == True:
-        #     self.phi = self.phi + self.delta  
-        # elif self.constant_speed == False:
-        #     if self.phi < self.slope:
-        #         self.phi = self.phi + self.velocity1
-        #     elif self.phi > 1 - self.slope:
-        #         self.phi = self.phi + self.velocity3
-        #     else:
-        #         self.phi = self.phi + self.velocity2
-
         #setting back to 0
         if self.phi>self.phi_end:
             self.phi = self.phi_start