diff --git a/scripts/motor_driver_node.py b/scripts/motor_driver_node.py
index 7636ab9..12c6764 100644
--- a/scripts/motor_driver_node.py
+++ b/scripts/motor_driver_node.py
@@ -540,17 +540,23 @@
 			if rospy.is_shutdown():
 				# -----Disable Motor-----
 				result = self.client.write_register(self.CONTROL_REG, self.DOWN_TIME, unit = self.ID)
-
 				break
 
 			start_time = time.perf_counter()
-			if self.estop == True:
+			if self.estop:
 				# -----Emergency Stop-----
-				result = self.client.write_register(self.CONTROL_REG, self.EMER_STOP, unit=self.ID)
+				if self.control_mode == 3:
+					self.set_rpm_with_limit(0, 0)
+				elif self.control_mode == 1:
+					result = self.client.write_register(self.CONTROL_REG, self.EMER_STOP, unit=self.ID)
 
+				if not estop_reset_flag:
+					rospy.logwarn("####################")
+					rospy.logwarn("---EMERGENCY STOP---")
+					rospy.logwarn("####################")
 				estop_reset_flag = True
-			elif self.estop == False:
-				if estop_reset_flag == True:
+			elif not self.estop:
+				if estop_reset_flag:
 					# -----Clear Alarm-----
 					result = self.client.write_register(self.CONTROL_REG, self.ALRM_CLR, unit=self.ID)