Added test data, correct debug message, Added emergency stop