adding registry

ReadMe.md

@@ -13,10 +13,20 @@ For detailed specifications, pinout, and connection diagrams, please refer to th
 
 ### Software dependences
 
-- [usb_i2c_adapter](https://devel.iri.upc.edu/pub/labrobotica/drivers/usb_i2c_adapter/trunk) C++ library
+- [iriutils](https://devel.iri.upc.edu/pub/labrobotica/drivers/iriutils/trunk) IRI C++ library
+
+  - Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/iriutils/trunk comm`
+  - Compile and install: `cd iriutils/build && cmake .. && make && sudo make install`
+
+- [comm](https://devel.iri.upc.edu/pub/labrobotica/drivers/comm/trunk) IRI C++ library
+
+  - Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/comm/trunk comm`
+  - Compile and install: `cd comm/build && cmake .. && make && sudo make install`
+
+- [usb_i2c_adapter](https://devel.iri.upc.edu/pub/labrobotica/drivers/usb_i2c_adapter/trunk) IRI C++ library
 
   - Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/usb_i2c_adapter/trunk usb_i2c_adapter`
-	- Compile and install: `cd usb_i2c_adapter/build && cmake .. && make && sudo make install`
+  - Compile and install: `cd usb_i2c_adapter/build && cmake .. && make && sudo make install`
 
 ### Example of usage
 

src/examples/lidar_lite_test.cpp

@@ -8,9 +8,12 @@ int main(int argc, char *argv[])
 
 	CLidarLite* laser_ptr = new CLidarLite(device_id,serial);
 
+	
 	try
 	{
 		laser_ptr->open();
+		laser_ptr->open();
+		laser_ptr->reset();
 		laser_ptr->close();
 	}catch(CLidarLiteException &e)
 	{

src/lidar_lite.cpp

@@ -4,6 +4,7 @@ CLidarLite::CLidarLite(const unsigned char &dev_id, const std::string &serial)
 {
   this->dev_id_ = dev_id;
   this->serial_ = serial;
+  this->status_ = IDLE;
 }
  
 CLidarLite::~CLidarLite(void)
@@ -11,35 +12,23 @@ CLidarLite::~CLidarLite(void)
 	close();
 }
 
-void CLidarLite::blink_led(void)
+void CLidarLite::write(unsigned char addr, unsigned char cmd)
 {
-	try
-	{
-		for (int ii = 0; ii < 2; ++ii)
-		{
-      this->adapter_->turn_led_off();
-      usleep(200000);
-      this->adapter_->turn_led_on();
-      usleep(200000);
-		}
-  }catch(CException &e)
-  {
-  	throw CLidarLiteException(_HERE_, e.what());
-  }
+  this->adapter_->write_reg(this->dev_id_, addr, &cmd, 1);
 }
 
-/*int CLidarLite::read(unsigned char &addr, int &len)
+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);
+  this->adapter_->write(this->dev_id_,&addr,1);
+  this->adapter_->read(this->dev_id_,data,len);
 
   int val;
   if (len==1)
-    val = (int)data;
+    val = data[0];
   else if (len==2)
   {
     unsigned short int read_val = data[0]*256+data[1];
@@ -48,13 +37,17 @@ void CLidarLite::blink_led(void)
 
   return val;
 }
-*/
+
 
 void CLidarLite::open(void)
 {
-	this->adapter_ = new CUSBI2C("adapter");
-	try
-	{
+  if (this->status_ != IDLE)
+    throw CLidarLiteException(_HERE_, "Device cannot be OPENNED because it is already running.");
+
+  try
+  {
+  	this->adapter_ = new CUSBI2C("adapter");
+
 		// open serial port
     std::cout << "[CLidarLite] Opening Lidar Lite. " << std::endl; 
     this->adapter_->open(this->serial_);
@@ -66,38 +59,68 @@ void CLidarLite::open(void)
     this->adapter_->config_gpio(gpio2,i2c);
     this->adapter_->config_gpio(gpio3,i2c);
 
+
     // *********************************************
 
-    unsigned char cmd = 0x04;
-    this->adapter_->write_reg(this->dev_id_, 0x00, &cmd, 1);
+    // unsigned char cmd = 0x04;
+    // this->adapter_->write_reg(this->dev_id_, 0x00, &cmd, 1);
+    write(ACQ_COMMAND,0x04);
+
+    // unsigned char add = 0x01;
+
+    // int data_read;
 
+    //Read register 0x01. Repeat until bit 0 (LSB) goes low.
     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 );
-
-    // 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_val = data[0]*256+data[1];
-
-    std::cout << read_val << " cm" << std::endl;
+    do 
+      busy = read(0x01,1);
+    while( busy & 0x01 );
+    
+
+    // regular read
+    // unsigned short int read_val;
+
+    // int len = 1;
+    // add = 0x04;
+    // unsigned char data_short[len];
+    // this->adapter_->write(this->dev_id_,&add,1);
+    // this->adapter_->read(this->dev_id_,data_short,len);
+
+    // read_val = data_short[0];
+
+    // std::cout << read_val << std::endl;
+
+    std::cout << read(0x04,1) << std::endl;
+
+    // double read
+
+    // len = 2;
+    // unsigned char data[len];
+    // add = 0x8f;
+    // this->adapter_->write(this->dev_id_,&add,1);
+    // this->adapter_->read(this->dev_id_,data,len);
+
+    // read_val = data[0]*256+data[1];
+
+    // std::cout << read_val << " cm" << std::endl;
+
+    std::cout << read(0x8f,2) << " cm" << std::endl;
     
     // ************************************************
+
   }catch(CException &e)
   {
   	throw CLidarLiteException(_HERE_, e.what());
   }
+  
+  this->status_ = RUNNING;
 }
 
 void CLidarLite::close(void)
 {
+  if (this->status_ != RUNNING)
+    throw CLidarLiteException(_HERE_, "Device cannot be CLOSED because it is not running.");
+
 	try
 	{
     if(this->adapter_ != NULL)
@@ -110,3 +133,29 @@ void CLidarLite::close(void)
   	throw CLidarLiteException(_HERE_, e.what());
   }
 }
+
+void CLidarLite::reset(void)
+{
+  if (this->status_ != RUNNING)
+    throw CLidarLiteException(_HERE_, "Device cannot be RESET because it is not running.");
+
+  write(ACQ_COMMAND,0x00);
+}
+
+
+void CLidarLite::blink_led(void)
+{
+  try
+  {
+    for (int ii = 0; ii < 2; ++ii)
+    {
+      this->adapter_->turn_led_off();
+      usleep(200000);
+      this->adapter_->turn_led_on();
+      usleep(200000);
+    }
+  }catch(CException &e)
+  {
+    throw CLidarLiteException(_HERE_, e.what());
+  }
+}

src/lidar_lite.h

@@ -5,16 +5,57 @@
 #include "usb_i2c.h"
 #include "exceptions/lidar_lite_exceptions.h"
 
+enum V3_REGISTER_DEF
+{  
+	ACQ_COMMAND = 		0x00,	// Device command
+	STATUS = 			0x01,	// System status
+	SIG_COUNT_VAL = 	0x02,	// Maximum acquisition count
+	ACQ_CONFIG_REG = 	0x04,	// Acquisition mode control
+	VELOCITY = 			0x09,	// Velocity measurement output
+	PEAK_CORR = 		0x0c,	// Peak value in correlation record	
+	NOISE_PEAK = 		0x0d,
+	SIGNAL_STRENGTH = 	0x0e,
+	FULL_DELAY_HIGH = 	0x0f,
+	FULL_DELAY_LOW = 	0x10,
+	OUTER_LOOP_COUNT = 	0x11,
+	REF_COUNT_VAL = 	0x12,
+	LAST_DELAY_HIGH = 	0x14,
+	LAST_DELAY_LOW = 	0x15,
+	UNIT_ID_HIGH = 		0x16,
+	UNIT_ID_LOW = 		0x17,
+	I2C_ID_HIGH = 		0x18,
+	I2C_ID_LOW = 		0x19,
+	I2C_SEC_ADDR = 		0x1a,
+	THRESHOLD_BYPASS = 	0x1c,
+	I2C_CONFIG = 		0x1e,
+	COMMAND = 			0x40,
+	MEASURE_DELAY = 	0x45,
+	PEAK_BCK = 			0x4c,
+	CORR_DATA = 		0x52,
+	CORR_DATA_SIGN = 	0x53,
+	ACQ_SETTINGS =		0x5d,
+	POWER_CONTROL =		0x65, 
+};
+
+enum DEV_STATUS 
+{  
+	IDLE,
+	RUNNING
+};
+
 class CLidarLite
 {
 	private:
-		std::string serial_;		// Serial port (e.g., run dmesg)
-		unsigned char dev_id_;	// Device ID (from datasheet)
-		CUSBI2C* adapter_;			// Device object
+		std::string serial_;	// Serial port (e.g., run dmesg).
+		unsigned char dev_id_;	// Device ID (from datasheet).
+		CUSBI2C* adapter_;		// Device object.
+		int status_;			// Device status.
 
 		void blink_led(void);
 
-		// int read(unsigned char &addr, int &len);
+
+		void write(unsigned char addr, unsigned char cmd);
+		int read(unsigned char addr, int len);
 
   public:
     CLidarLite(const unsigned char &dev_id, const std::string &serial);
@@ -22,6 +63,7 @@ class CLidarLite
 
 		void open(void);
 		void close(void);
+		void reset(void);
 };
 
 #endif