diff --git a/src/examples/lidar_lite_test.cpp b/src/examples/lidar_lite_test.cpp
index eaf7009463cae8e4c8c22c7363a7423e6655817b..e7f6b1f245545f6b85ba5ba259e3ab63ec716eb4 100644
--- a/src/examples/lidar_lite_test.cpp
+++ b/src/examples/lidar_lite_test.cpp
@@ -4,13 +4,13 @@ int main(int argc, char *argv[])
 {
 
 	std::string serial="A700evSl";
+	unsigned char device_id=0x62;
 
-	CLidarLite* laser_ptr = new CLidarLite();
+	CLidarLite* laser_ptr = new CLidarLite(device_id,serial);
 
 	try
 	{
-		laser_ptr->open(serial);
-
+		laser_ptr->open();
 		laser_ptr->close();
 	}catch(CLidarLiteException &e)
 	{
@@ -18,5 +18,8 @@ int main(int argc, char *argv[])
 	}
 
 	if(laser_ptr != NULL)
+	{
+		laser_ptr = NULL;
   	delete laser_ptr;
+	}
 }
diff --git a/src/lidar_lite.cpp b/src/lidar_lite.cpp
index d99f788f995e661e9b7c3440145f0c694db2462b..889e3540ed6eb097eccbbab195f8250ca29b6cbb 100644
--- a/src/lidar_lite.cpp
+++ b/src/lidar_lite.cpp
@@ -1,14 +1,14 @@
 #include "lidar_lite.h"
 
-CLidarLite::CLidarLite(void)
+CLidarLite::CLidarLite(const unsigned char &dev_id, const std::string &serial)
 {
+  this->dev_id_ = dev_id;
+  this->serial_ = serial;
 }
  
 CLidarLite::~CLidarLite(void)
 {
 	close();
-	if(this->adapter_ != NULL)
-		delete this->adapter_;
 }
 
 void CLidarLite::blink_led(void)
@@ -28,43 +28,68 @@ void CLidarLite::blink_led(void)
   }
 }
 
-void CLidarLite::open(const std::string& serial)
+/*int CLidarLite::read(unsigned char &addr, int &len)
+{
+  if (len!=1 && len!=2)
+    throw CLidarLiteException(_HERE_, "Expected read data with invalid length. This function only allows to read 1 or 2 bytes");
+
+  unsigned char data[len];
+  this->adapter_->write(this->dev_id_,&addr,len);
+  this->adapter_->read(this->dev_id_,&data,len);
+
+  int val;
+  if (len==1)
+    val = (int)data;
+  else if (len==2)
+  {
+    unsigned short int read_val = data[0]*256+data[1];
+    val = (int)read_val;
+  }
+
+  return val;
+}
+*/
+
+void CLidarLite::open(void)
 {
 	this->adapter_ = new CUSBI2C("adapter");
 	try
 	{
 		// open serial port
     std::cout << "[CLidarLite] Opening Lidar Lite. " << std::endl; 
-    this->adapter_->open(serial);
-    std::cout << "  - Serial number: " << serial << std::endl;
+    this->adapter_->open(this->serial_);
+    usleep(10000); // Avoid initialization errors
+    std::cout << "  - Serial number: " << this->serial_ << std::endl;
     std::cout << "  - Firmware revision: " << (int)this->adapter_->get_revision() << std::endl;
 
-    // blink led
-    blink_led();
-		
-		// Setup gpio 2 as i2c    
+		// Setup gpio 2 and 3 as i2c    
     this->adapter_->config_gpio(gpio2,i2c);
+    this->adapter_->config_gpio(gpio3,i2c);
 
-    // Test
-
-		
-		// unsigned char param1 = 0x00;
-		unsigned char data = 0x04;
-
-    this->adapter_->write_reg(gpio2, 0x00, &data, 1);
-
-		// this->adapter_->write(0x00, &data, 1);
+    // *********************************************
 
-		//int CUSBI2C::write_reg(unsigned char dev_id, short int address, unsigned char *data, int len)
-  
+    unsigned char cmd = 0x04;
+    this->adapter_->write_reg(this->dev_id_, 0x00, &cmd, 1);
 
+    unsigned char busy;
+    unsigned char add = 0x01;
+    do {
+      this->adapter_->write(this->dev_id_,&add,1);
+      this->adapter_->read(this->dev_id_,&busy,1);
+    }while( busy & 0x01 );
 
-  //Write 0x04 to register 0x00.
+    // unsigned short int data;
+    unsigned char data[2];
+    unsigned short int read_val;
+    add = 0x8f;
+    this->adapter_->write(this->dev_id_,&add,1);
+    this->adapter_->read(this->dev_id_,data,2);
 
-	//Read register 0x01. Repeat until bit 0 (LSB) goes low.
-			
-	//Read two bytes from 0x8f (High byte 0x0f then low byte 0x10) to obtain the 16-bit measured distance in centimeter
+    read_val = data[0]*256+data[1];
 
+    std::cout << read_val << " cm" << std::endl;
+    
+    // ************************************************
   }catch(CException &e)
   {
   	throw CLidarLiteException(_HERE_, e.what());
@@ -75,7 +100,11 @@ void CLidarLite::close(void)
 {
 	try
 	{
-		this->adapter_->close();
+    if(this->adapter_ != NULL)
+    {
+      this->adapter_ = NULL;
+      delete this->adapter_;    
+    }
   }catch(CException &e)
   {
   	throw CLidarLiteException(_HERE_, e.what());
diff --git a/src/lidar_lite.h b/src/lidar_lite.h
index da9c649219456dd1dfb5134118d486ca96b38bda..14500c1db6b4cfaea4ac7abefb250a95491607a3 100644
--- a/src/lidar_lite.h
+++ b/src/lidar_lite.h
@@ -8,16 +8,19 @@
 class CLidarLite
 {
 	private:
-		std::string serial_;	// Serial port
+		std::string serial_;		// Serial port (e.g., run dmesg)
+		unsigned char dev_id_;	// Device ID (from datasheet)
 		CUSBI2C* adapter_;			// Device object
 
 		void blink_led(void);
 
+		// int read(unsigned char &addr, int &len);
+
   public:
-    CLidarLite(void);
+    CLidarLite(const unsigned char &dev_id, const std::string &serial);
     ~CLidarLite(void);
 
-		void open(const std::string& serial);
+		void open(void);
 		void close(void);
 };