Steven Ventura / LidarLitev2

LidarLitev2 Library for distance reading. Capable of both single distance reads and continuous distance read setup.

Dependents:   Lidar_Distance Lidar_DistanceContinuous Lidar_2D_Mapping Motions_Secure_Server_IUPUI ... more

LidarLitev2.cpp@1:238f6a0108e7, 2015-10-22 (annotated)

Committer:
sventura3
Date:
Thu Oct 22 23:49:53 2015 +0000
Revision:
1:238f6a0108e7
Parent:
0:7e6c07be1754 
Commenting

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sventura3 0:7e6c07be1754 1 #include "LidarLitev2.h"
sventura3 0:7e6c07be1754 2 #include "math.h"
sventura3 0:7e6c07be1754 3
sventura3 0:7e6c07be1754 4 int nackack;
sventura3 0:7e6c07be1754 5 //Constructor iniating the I2C connection
sventura3 0:7e6c07be1754 6 LidarLitev2::LidarLitev2(PinName sda, PinName scl, bool fast) : i2c(sda, scl)
sventura3 0:7e6c07be1754 7 {
sventura3 0:7e6c07be1754 8 if (fast) i2c.frequency(400000); //I2C @ 400kHz
sventura3 0:7e6c07be1754 9 else i2c.frequency(100000); //I2C @ 100Khz
sventura3 0:7e6c07be1754 10 wait(0.2);
sventura3 0:7e6c07be1754 11 }
sventura3 0:7e6c07be1754 12
sventura3 0:7e6c07be1754 13 void LidarLitev2::configure(int config, int LidarLitev2_addr)
sventura3 0:7e6c07be1754 14 {
sventura3 0:7e6c07be1754 15 char reset[2] = {0x00, 0x00}; //Register and value to Reset the Lidar to orgional settings
sventura3 0:7e6c07be1754 16 char aquisition[2] = {0x04, 0x00}; //Register and value to set the acquisition to 1/3 default value
sventura3 1:238f6a0108e7 17 switch (config) {
sventura3 0:7e6c07be1754 18 case 0: //Deafault config
sventura3 0:7e6c07be1754 19 nackack = 1;
sventura3 1:238f6a0108e7 20 while(nackack !=0) {
sventura3 0:7e6c07be1754 21 //wait_ms(1);
sventura3 0:7e6c07be1754 22 nackack = i2c.write(LidarLitev2_addr, reset, 2); // Resets the Lidar settings
sventura3 0:7e6c07be1754 23 }
sventura3 1:238f6a0108e7 24
sventura3 0:7e6c07be1754 25 break;
sventura3 0:7e6c07be1754 26 case 1:
sventura3 0:7e6c07be1754 27 nackack = 1;
sventura3 1:238f6a0108e7 28 while(nackack !=0) {
sventura3 0:7e6c07be1754 29 //wait_ms(1);
sventura3 0:7e6c07be1754 30 nackack = i2c.write(LidarLitev2_addr, aquisition, 2); // Set the acquisition to 1/3 default value
sventura3 0:7e6c07be1754 31 }
sventura3 0:7e6c07be1754 32 break;
sventura3 1:238f6a0108e7 33 }
sventura3 0:7e6c07be1754 34 }
sventura3 0:7e6c07be1754 35
sventura3 0:7e6c07be1754 36 void LidarLitev2::beginContinuous(bool modePinLow, char interval, char numberOfReadings,int LidarLitev2_addr)
sventura3 0:7e6c07be1754 37 {
sventura3 0:7e6c07be1754 38 char reg_freq[2] = {0x45, interval}; // Sets the time between measurements through reg 0x45
sventura3 1:238f6a0108e7 39
sventura3 0:7e6c07be1754 40 nackack = 1;
sventura3 1:238f6a0108e7 41 while(nackack !=0) {
sventura3 1:238f6a0108e7 42 nackack =i2c.write(LidarLitev2_addr, reg_freq, 2, false); //Write to register 0x45 the interval time
sventura3 1:238f6a0108e7 43 }
sventura3 1:238f6a0108e7 44 // Set register 0x04 to 0x20 to look at "NON-default" value of velocity scale
sventura3 1:238f6a0108e7 45 // If you set bit 0 of 0x04 to "1" then the mode pin will be low when done
sventura3 0:7e6c07be1754 46 char reg_modePin[2] = {0x04, 0x21}; // Default modePin Setting
sventura3 0:7e6c07be1754 47 if (!modePinLow) reg_modePin[1] = 0x20;
sventura3 1:238f6a0108e7 48
sventura3 0:7e6c07be1754 49 nackack = 1;
sventura3 1:238f6a0108e7 50 while(nackack !=0) {
sventura3 1:238f6a0108e7 51 nackack =i2c.write(LidarLitev2_addr, reg_modePin, 2, false);
sventura3 1:238f6a0108e7 52 }
sventura3 0:7e6c07be1754 53 char reg_numOfReadings[2] = {0x11, numberOfReadings}; // Set the number of readings through reg 0x11
sventura3 1:238f6a0108e7 54
sventura3 0:7e6c07be1754 55 nackack = 1;
sventura3 1:238f6a0108e7 56 while(nackack !=0) {
sventura3 1:238f6a0108e7 57 nackack =i2c.write(LidarLitev2_addr, reg_numOfReadings, 2, false); // writes into Reg_numofreadings the number of readings desired
sventura3 1:238f6a0108e7 58 }
sventura3 0:7e6c07be1754 59 char reg_reading_distance[2] = {0x00, 0x04}; // Iniates reg_readDistance
sventura3 0:7e6c07be1754 60 nackack = 1;
sventura3 1:238f6a0108e7 61 while(nackack !=0) {
sventura3 1:238f6a0108e7 62 nackack =i2c.write(LidarLitev2_addr, reg_reading_distance, 2, false); // writes into Reg_readDistance to begin continuos reading
sventura3 1:238f6a0108e7 63 }
sventura3 0:7e6c07be1754 64 }
sventura3 0:7e6c07be1754 65
sventura3 1:238f6a0108e7 66 int LidarLitev2::distance(bool stablizePreampFlag, bool takeReference, int LidarLitev2_addr)
sventura3 1:238f6a0108e7 67 {
sventura3 0:7e6c07be1754 68 // Standard read distance protocol without continuous
sventura3 1:238f6a0108e7 69
sventura3 0:7e6c07be1754 70 char reg_dc[2] = {0x00, 0x04}; // Iniates reg_readDistance
sventura3 0:7e6c07be1754 71 if(!stablizePreampFlag) reg_dc[1] = 0x03;
sventura3 1:238f6a0108e7 72
sventura3 0:7e6c07be1754 73 nackack = 1;
sventura3 1:238f6a0108e7 74 while(nackack !=0) {
sventura3 0:7e6c07be1754 75 nackack = i2c.write(LidarLitev2_addr, reg_dc, 2); // writes into Reg_readDistance to begin a reading
sventura3 0:7e6c07be1754 76 }
sventura3 1:238f6a0108e7 77
sventura3 0:7e6c07be1754 78 char data[2];
sventura3 0:7e6c07be1754 79 char reg_read[1] = {0x8f}; // Register to read distance value from
sventura3 1:238f6a0108e7 80
sventura3 0:7e6c07be1754 81 nackack = 1;
sventura3 1:238f6a0108e7 82 while(nackack !=0) {
sventura3 0:7e6c07be1754 83 nackack = i2c.write(LidarLitev2_addr, reg_read, 1); // Ready to read from reg distance
sventura3 0:7e6c07be1754 84 }
sventura3 1:238f6a0108e7 85
sventura3 0:7e6c07be1754 86 nackack = 1;
sventura3 1:238f6a0108e7 87 while(nackack !=0) {
sventura3 0:7e6c07be1754 88 nackack = i2c.read(LidarLitev2_addr, data, 2); // Read from reg distance 2bytes of information
sventura3 0:7e6c07be1754 89 }
sventura3 0:7e6c07be1754 90 int distance = ((uint8_t)data[0]<<8) + (uint8_t)data[1]; //Combine byes to a int to be returned as the distance measured
sventura3 0:7e6c07be1754 91 return distance;
sventura3 0:7e6c07be1754 92 }
sventura3 0:7e6c07be1754 93
sventura3 0:7e6c07be1754 94 int LidarLitev2::distanceContinuous(int LidarLitev2_addr)
sventura3 0:7e6c07be1754 95 {
sventura3 0:7e6c07be1754 96 char data[2];
sventura3 0:7e6c07be1754 97 char reg_read[1] = {0x8f}; // Register to read distance value from
sventura3 1:238f6a0108e7 98
sventura3 0:7e6c07be1754 99 nackack = 1;
sventura3 1:238f6a0108e7 100 while(nackack !=0) {
sventura3 1:238f6a0108e7 101 nackack =i2c.write(LidarLitev2_addr, reg_read, 1); // Ready to read from reg distance
sventura3 1:238f6a0108e7 102 }
sventura3 0:7e6c07be1754 103 nackack = 1;
sventura3 1:238f6a0108e7 104 while(nackack !=0) {
sventura3 1:238f6a0108e7 105 nackack =i2c.read(LidarLitev2_addr, data, 2); // Read from reg distance 2bytes of information
sventura3 1:238f6a0108e7 106 }
sventura3 0:7e6c07be1754 107 int distance = ((uint8_t)data[0]<<8) + (uint8_t)data[1]; //Combine byes to a int to be returned as the distance measured
sventura3 0:7e6c07be1754 108 return distance;
sventura3 0:7e6c07be1754 109 }