Diff: LidarLite.cpp

Revision:

1:b8d3e9e59703

Parent:

0:8e6304ab38d2

--- a/LidarLite.cpp	Tue Feb 17 12:01:04 2015 +0000
+++ b/LidarLite.cpp	Thu Jun 16 10:26:33 2016 +0000
@@ -12,9 +12,15 @@
 LidarLite::LidarLite(PinName sda, PinName scl)
 {
     i2c_ = new I2C(sda, scl);
-    i2c_->frequency(100000); //I2C @ 100kHz
+    i2c_->frequency(400000); //I2C @ 100kHz
     wait(0.5);
 }
+LidarLite::LidarLite(PinName sda, PinName scl, char currentAddress)
+{
+    i2c_ = new I2C(sda, scl);
+    i2c_->frequency(400000); //I2C @ 100kHz
+    wait(0.5);    
+}  
 
 int16_t LidarLite::getRange_cm()
 {
@@ -29,12 +35,48 @@
         return((velocity_LL-256)*10);
 }
 
+void LidarLite::refreshRangeAdd(char currentAddres)
+{
+    uint8_t nackack;
+    
+    char write[2]={SET_CommandReg, AcqMode};//00,0x04
+    char read_dist[1]={GET_Distance2BReg};//0x8f
+    char read_vel[1]={GET_VelocityReg};
+    
+    char dist[2];
+    char vel[1];
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        //nackack = i2c_->write(LIDARLite_WriteAdr, write, 2);
+        nackack = i2c_->write((currentAddres<<1), write, 2);
+    }
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+//        nackack = i2c_->write(LIDARLite_WriteAdr, read_dist, 1);
+        nackack = i2c_->write((currentAddres<<1), read_dist, 1);
+    }    
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+            nackack = i2c_->read(((currentAddres<<1)|0X01), dist, 2);
+//        nackack = i2c_->read(LIDARLite_ReadAdr, dist, 2);
+    }
+    distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1];
+}
 void LidarLite::refreshRange()
 {
     uint8_t nackack;
     
-    char write[2]={SET_CommandReg, AcqMode};
-    char read_dist[1]={GET_Distance2BReg};
+    char write[2]={SET_CommandReg, AcqMode};//00,0x04
+    char read_dist[1]={GET_Distance2BReg};//0x8f
     char read_vel[1]={GET_VelocityReg};
     
     char dist[2];
@@ -62,7 +104,6 @@
     }
     distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1];
 }
-
 void LidarLite::refreshVelocity()
 {
     uint8_t nackack;
@@ -96,6 +137,41 @@
     }
     velocity_LL = (uint16_t)vel[0];
 }
+//For V2 sensor
+
+void LidarLite::refreshVelocityAdd(char currentAddres)
+{
+    uint8_t nackack;
+    
+    char write[2]={SET_CommandReg, AcqMode};
+    char read_dist[1]={GET_Distance2BReg};
+    char read_vel[1]={GET_VelocityReg};
+    
+    char dist[2];
+    char vel[1];
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        nackack = i2c_->write((currentAddres<<1), write, 2);
+    }
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        nackack = i2c_->write((currentAddres<<1), read_vel, 1);
+    }    
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        nackack = i2c_->read(((currentAddres<<1)|0x01), vel, 1);
+    }
+    velocity_LL = (uint16_t)vel[0];
+}
 
 void LidarLite::refreshRangeVelocity()
 {
@@ -145,4 +221,114 @@
         nackack = i2c_->read(LIDARLite_ReadAdr, vel, 1);
     }
     velocity_LL = (uint16_t)vel[0];
+}
+
+//For V2
+void LidarLite::refreshRangeVelocityAdd(char currentAddres)
+{
+    uint8_t nackack;
+    
+    char write[2]={SET_CommandReg, AcqMode};
+    char read_dist[1]={GET_Distance2BReg};
+    char read_vel[1]={GET_VelocityReg};
+    
+    char dist[2];
+    char vel[1];
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        nackack = i2c_->write((currentAddres<<1), write, 2);
+    }
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        nackack = i2c_->write((currentAddres<<1), read_dist, 1);
+    }    
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        nackack = i2c_->read(((currentAddres<<1)|0x01), dist, 2);
+    }
+    distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1];
+    
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        nackack = i2c_->write((currentAddres<<1), read_vel, 1);
+    }    
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+        wait_ms(1);
+        nackack = i2c_->read(((currentAddres<<1)|0x01), vel, 1);
+    }
+    velocity_LL = (uint16_t)vel[0];
+}
+
+void LidarLite::changeAddress(char currentAddres,char newAddress)
+{   
+
+    char read_vel[1]={0x96};
+    char write[2]={0x1a,};
+    char write_sno[2]={0x18,};
+    char write_sno1[2]={0x19,};
+    char write_D[2]={0x1e,0x08}; //disable Primary Address
+    write[1]= newAddress;
+
+    char s_no[2];
+    uint8_t nackack=1;
+
+    while(nackack !=0)
+    {
+            wait_ms(1);
+            nackack = i2c_->read(((currentAddres<<1)|0x01), s_no, 2);
+    }
+    
+    
+    write_sno[1]=s_no[0];
+    write_sno1[1]=s_no[0];
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+            wait_ms(1);
+             i2c_->write((currentAddres<<1), write_sno, 2);//write 0x18
+    }
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+            wait_ms(1);
+             i2c_->write((currentAddres<<1), write_sno1, 2);//write 0x18
+    }
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+            wait_ms(1);
+             i2c_->write((currentAddres<<1), write, 2);
+    }
+    
+    nackack=1;
+    while(nackack !=0)
+    {
+            wait_ms(1);
+            i2c_->write((currentAddres<<1), write, 2);
+    }
+    nackack=1;
+    while(nackack !=0)
+    {
+            wait_ms(1);
+            i2c_->write((currentAddres<<1), write_D, 2);
+    }
+    
 }
\ No newline at end of file