diff --git a/src/autopilot.cpp b/src/autopilot.cpp
index 89f6897..841dd3f 100644
--- a/src/autopilot.cpp
+++ b/src/autopilot.cpp
@@ -1,21 +1,90 @@
 #include <ros/ros.h>
 #include "std_msgs/String.h"
+#include <std_msgs/Float64.h>
+#include <sensor_msgs/BatteryState.h>
 #include <geometry_msgs/Twist.h>
+#include <sensor_msgs/LaserScan.h>
 
+// Parameters
+bool isCollisionDetected = false;
+bool isFinished = false;
+int angleDetection = 15;
+double distThreshold = 0.3;
+float batThreshold = 10;
+double entroThreshold = 100000000;
+
+// Function to run when receive data from the subscribed Lidar
+void scanCallback(const sensor_msgs::LaserScan::ConstPtr& data) //
+{
+	isCollisionDetected = false;
+	
+	for (int i=0;i<angleDetection;i++){
+		
+		// Get data in a cone of +-15° in front of the robot 
+		double firstVal = data->ranges[i];
+		double lastVal = data->ranges[data->ranges.size()-i-1];
+		
+		// Check if one of the data is below the threshold
+		if (lastVal <distThreshold && lastVal != 0.0){
+			isCollisionDetected = true;
+			break;
+		}
+		if (firstVal <distThreshold&& firstVal != 0.0){
+			isCollisionDetected = true;
+			break;
+		}
+	}
+	
+}
+
+void entropyCallback(const std_msgs::Float64& entropy) //??
+{
+	if (entropy.data > entroThreshold){
+		isFinished = true;
+	}
+}
+
+void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& bat)
+{
+	if (bat->voltage < batThreshold){
+		isFinished = true;
+	}
+}
+	
+	
 int main(int argc, char** argv)
 {
 	ros::init(argc,argv, "autopilot");
 	
-	// Subscribe to the cmd_vel topic
+	// Publish to the cmd_vel topic
 	ros::NodeHandle nh;
 	ros::Publisher cmdVelPub = nh.advertise<geometry_msgs::Twist>("cmd_vel",1);
-    
+	
+	// Subscribe to the scan topic
+	ros::Subscriber scanSub = nh.subscribe("scan",1,scanCallback); 
+	
+	// Subscribe to the entropy topic
+	ros::Subscriber entropySub = nh.subscribe("turtlebot3_slam_gmapping/entropy",1,entropyCallback); 
+	
+	// Subscribe to the battery state topic
+	ros::Subscriber batterySub = nh.subscribe("battery_state",1,batteryCallback); 
+	
 	ros::Rate loopRate(10);
     
 	while(ros::ok())
 	{
 		geometry_msgs::Twist msg;
-		msg.angular.z =1;
+		
+		
+		if (!isFinished){
+			if (isCollisionDetected){
+				msg.angular.z = 1;
+			}
+			else{
+				msg.linear.x = 1;
+			}
+		}
+		
 		cmdVelPub.publish(msg);
 		ros::spinOnce();
 		loopRate.sleep();