LidarLitev2.cpp

00001 #include "LidarLitev2.h"
00002 #include "math.h"
00003 
00004 int nackack;
00005 //Constructor iniating the I2C connection
00006 LidarLitev2::LidarLitev2(PinName sda, PinName scl, bool fast) : i2c(sda, scl)
00007 {
00008     if (fast) i2c.frequency(400000); //I2C @ 400kHz
00009     else i2c.frequency(100000);     //I2C @ 100Khz
00010     wait(0.2);
00011 }
00012 
00013 void LidarLitev2::configure(int config, int LidarLitev2_addr)
00014 {
00015     char reset[2] = {0x00, 0x00};           //Register and value to Reset the Lidar to orgional settings
00016     char aquisition[2] = {0x04, 0x00};      //Register and value to set the acquisition to 1/3 default value
00017     switch (config) {
00018         case 0: //Deafault config
00019             nackack = 1;
00020             while(nackack !=0) {
00021                 //wait_ms(1);
00022                 nackack = i2c.write(LidarLitev2_addr, reset, 2); // Resets the Lidar settings
00023             }
00024 
00025             break;
00026         case 1:
00027             nackack = 1;
00028             while(nackack !=0) {
00029                 //wait_ms(1);
00030                 nackack = i2c.write(LidarLitev2_addr, aquisition, 2);  // Set the acquisition to 1/3 default value
00031             }
00032             break;
00033     }
00034 }
00035 
00036 void LidarLitev2::beginContinuous(bool modePinLow, char interval, char numberOfReadings,int LidarLitev2_addr)
00037 {
00038     char reg_freq[2] = {0x45, interval};    // Sets the time between measurements through reg 0x45
00039 
00040     nackack = 1;
00041     while(nackack !=0) {
00042         nackack =i2c.write(LidarLitev2_addr, reg_freq, 2, false);  //Write to register 0x45 the interval time
00043     }
00044     //  Set register 0x04 to 0x20 to look at "NON-default" value of velocity scale
00045     //  If you set bit 0 of 0x04 to "1" then the mode pin will be low when done
00046     char reg_modePin[2] = {0x04, 0x21}; // Default modePin Setting
00047     if (!modePinLow) reg_modePin[1] = 0x20;
00048 
00049     nackack = 1;
00050     while(nackack !=0) {
00051         nackack =i2c.write(LidarLitev2_addr, reg_modePin, 2, false);
00052     }
00053     char reg_numOfReadings[2] = {0x11, numberOfReadings};  // Set the number of readings through reg 0x11
00054 
00055     nackack = 1;
00056     while(nackack !=0) {
00057         nackack =i2c.write(LidarLitev2_addr, reg_numOfReadings, 2, false);   // writes into Reg_numofreadings the number of readings desired
00058     }
00059     char reg_reading_distance[2] = {0x00, 0x04};  // Iniates reg_readDistance
00060     nackack = 1;
00061     while(nackack !=0) {
00062         nackack =i2c.write(LidarLitev2_addr, reg_reading_distance, 2, false);   // writes into Reg_readDistance to begin continuos reading
00063     }
00064 }
00065 
00066 int LidarLitev2::distance(bool stablizePreampFlag, bool takeReference, int LidarLitev2_addr)
00067 {
00068     // Standard read distance protocol without continuous
00069 
00070     char reg_dc[2] = {0x00, 0x04};  // Iniates reg_readDistance
00071     if(!stablizePreampFlag) reg_dc[1] = 0x03;
00072 
00073     nackack = 1;
00074     while(nackack !=0) {
00075         nackack = i2c.write(LidarLitev2_addr, reg_dc, 2); // writes into Reg_readDistance to begin a reading
00076     }
00077 
00078     char data[2];
00079     char reg_read[1] = {0x8f};  // Register to read distance value from
00080 
00081     nackack = 1;
00082     while(nackack !=0) {
00083         nackack = i2c.write(LidarLitev2_addr, reg_read, 1); // Ready to read from reg distance
00084     }
00085 
00086     nackack = 1;
00087     while(nackack !=0) {
00088         nackack = i2c.read(LidarLitev2_addr, data, 2);  // Read from reg distance 2bytes of information
00089     }
00090     int distance = ((uint8_t)data[0]<<8) + (uint8_t)data[1]; //Combine byes to a int to be returned as the distance measured
00091     return distance;
00092 }
00093 
00094 int LidarLitev2::distanceContinuous(int LidarLitev2_addr)
00095 {
00096     char data[2];
00097     char reg_read[1] = {0x8f}; // Register to read distance value from
00098 
00099     nackack = 1;
00100     while(nackack !=0) {
00101         nackack =i2c.write(LidarLitev2_addr, reg_read, 1); // Ready to read from reg distance
00102     }
00103     nackack = 1;
00104     while(nackack !=0) {
00105         nackack =i2c.read(LidarLitev2_addr, data, 2); // Read from reg distance 2bytes of information
00106     }
00107     int distance = ((uint8_t)data[0]<<8) + (uint8_t)data[1]; //Combine byes to a int to be returned as the distance measured
00108     return distance;
00109 }