diff --git a/rmp220_middleware/rmp220_middleware.py b/rmp220_middleware/rmp220_middleware.py
index 613ff91..455f8bb 100644
--- a/rmp220_middleware/rmp220_middleware.py
+++ b/rmp220_middleware/rmp220_middleware.py
@@ -124,8 +124,8 @@ class StateMachineNode(Node):
         self.timeout = 20.0
 
     def timer_callback(self):
-        # if self.chassis_mode == State.DISABLED or self.chassis_mode == State.STOPPED or self.chassis_mode == State.PASSIVE:
-        #     return # Do nothing if chassis is disabled, stopped or passive --> should save processing power
+        if self.chassis_mode == State.DISABLED or self.chassis_mode == State.STOPPED or self.chassis_mode == State.PASSIVE:
+            return # Do nothing if chassis is disabled, stopped or passive --> should save processing power
         if self.state == State.ENABLED:
             if self.timeout <= 0:
                 self.state = State.PAUSED
@@ -139,8 +139,8 @@ class StateMachineNode(Node):
             self.state = State.ENABLED
             self.get_logger().info("State: ENABLED (cmd_vel)")
             self.enable_chassis()
-        if self.state == State.PAUSED and (self.abs_x < 0.1 and self.abs_z < 0.1): # Is this even necessary?
-            self.cmd_vel_pub.publish(self.twist)
+        # if self.state == State.PAUSED and (self.abs_x < 0.1 and self.abs_z < 0.1): # Is this even necessary?
+        #     self.cmd_vel_pub.publish(self.twist)
 
 def main(args=None):
     rclpy.init(args=args)