Robotics Interest Club / Mbed 2 deprecated LidarLite

Suitable for Lidar Lite V2

Dependencies:   mbed

Fork of LidarLite by Akash Vibhute

LidarLite.cpp@1:b8d3e9e59703, 2016-06-16 (annotated)

Committer:
thef
Date:
Thu Jun 16 10:26:33 2016 +0000
Revision:
1:b8d3e9e59703
Parent:
0:8e6304ab38d2 
Lidar Lite V2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 0:8e6304ab38d2 1 /*
akashvibhute 0:8e6304ab38d2 2 * Library for easy interface of LidarLite with mbed using I2C
akashvibhute 0:8e6304ab38d2 3 *
akashvibhute 0:8e6304ab38d2 4 * Akash Vibhute <akash . roboticist [at] gmail . com>
akashvibhute 0:8e6304ab38d2 5 *
akashvibhute 0:8e6304ab38d2 6 * v0.1, 17/Feb/2015 - First version of library, tested using LPC1768 [powered via mbed 3.3v, no additional pullups on I2C necessary]
akashvibhute 0:8e6304ab38d2 7 *
akashvibhute 0:8e6304ab38d2 8 */
akashvibhute 0:8e6304ab38d2 9
akashvibhute 0:8e6304ab38d2 10 #include "LidarLite.h"
akashvibhute 0:8e6304ab38d2 11
akashvibhute 0:8e6304ab38d2 12 LidarLite::LidarLite(PinName sda, PinName scl)
akashvibhute 0:8e6304ab38d2 13 {
akashvibhute 0:8e6304ab38d2 14 i2c_ = new I2C(sda, scl);
thef 1:b8d3e9e59703 15 i2c_->frequency(400000); //I2C @ 100kHz
akashvibhute 0:8e6304ab38d2 16 wait(0.5);
akashvibhute 0:8e6304ab38d2 17 }
thef 1:b8d3e9e59703 18 LidarLite::LidarLite(PinName sda, PinName scl, char currentAddress)
thef 1:b8d3e9e59703 19 {
thef 1:b8d3e9e59703 20 i2c_ = new I2C(sda, scl);
thef 1:b8d3e9e59703 21 i2c_->frequency(400000); //I2C @ 100kHz
thef 1:b8d3e9e59703 22 wait(0.5);
thef 1:b8d3e9e59703 23 }
akashvibhute 0:8e6304ab38d2 24
akashvibhute 0:8e6304ab38d2 25 int16_t LidarLite::getRange_cm()
akashvibhute 0:8e6304ab38d2 26 {
akashvibhute 0:8e6304ab38d2 27 return(distance_LL);
akashvibhute 0:8e6304ab38d2 28 }
akashvibhute 0:8e6304ab38d2 29
akashvibhute 0:8e6304ab38d2 30 int16_t LidarLite::getVelocity_cms()
akashvibhute 0:8e6304ab38d2 31 {
akashvibhute 0:8e6304ab38d2 32 if(velocity_LL < 127)
akashvibhute 0:8e6304ab38d2 33 return(velocity_LL*10);
akashvibhute 0:8e6304ab38d2 34 else
akashvibhute 0:8e6304ab38d2 35 return((velocity_LL-256)*10);
akashvibhute 0:8e6304ab38d2 36 }
akashvibhute 0:8e6304ab38d2 37
thef 1:b8d3e9e59703 38 void LidarLite::refreshRangeAdd(char currentAddres)
thef 1:b8d3e9e59703 39 {
thef 1:b8d3e9e59703 40 uint8_t nackack;
thef 1:b8d3e9e59703 41
thef 1:b8d3e9e59703 42 char write[2]={SET_CommandReg, AcqMode};//00,0x04
thef 1:b8d3e9e59703 43 char read_dist[1]={GET_Distance2BReg};//0x8f
thef 1:b8d3e9e59703 44 char read_vel[1]={GET_VelocityReg};
thef 1:b8d3e9e59703 45
thef 1:b8d3e9e59703 46 char dist[2];
thef 1:b8d3e9e59703 47 char vel[1];
thef 1:b8d3e9e59703 48
thef 1:b8d3e9e59703 49 nackack=1;
thef 1:b8d3e9e59703 50 while(nackack !=0)
thef 1:b8d3e9e59703 51 {
thef 1:b8d3e9e59703 52 wait_ms(1);
thef 1:b8d3e9e59703 53 //nackack = i2c_->write(LIDARLite_WriteAdr, write, 2);
thef 1:b8d3e9e59703 54 nackack = i2c_->write((currentAddres<<1), write, 2);
thef 1:b8d3e9e59703 55 }
thef 1:b8d3e9e59703 56
thef 1:b8d3e9e59703 57 nackack=1;
thef 1:b8d3e9e59703 58 while(nackack !=0)
thef 1:b8d3e9e59703 59 {
thef 1:b8d3e9e59703 60 wait_ms(1);
thef 1:b8d3e9e59703 61 // nackack = i2c_->write(LIDARLite_WriteAdr, read_dist, 1);
thef 1:b8d3e9e59703 62 nackack = i2c_->write((currentAddres<<1), read_dist, 1);
thef 1:b8d3e9e59703 63 }
thef 1:b8d3e9e59703 64
thef 1:b8d3e9e59703 65 nackack=1;
thef 1:b8d3e9e59703 66 while(nackack !=0)
thef 1:b8d3e9e59703 67 {
thef 1:b8d3e9e59703 68 wait_ms(1);
thef 1:b8d3e9e59703 69 nackack = i2c_->read(((currentAddres<<1)|0X01), dist, 2);
thef 1:b8d3e9e59703 70 // nackack = i2c_->read(LIDARLite_ReadAdr, dist, 2);
thef 1:b8d3e9e59703 71 }
thef 1:b8d3e9e59703 72 distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1];
thef 1:b8d3e9e59703 73 }
akashvibhute 0:8e6304ab38d2 74 void LidarLite::refreshRange()
akashvibhute 0:8e6304ab38d2 75 {
akashvibhute 0:8e6304ab38d2 76 uint8_t nackack;
akashvibhute 0:8e6304ab38d2 77
thef 1:b8d3e9e59703 78 char write[2]={SET_CommandReg, AcqMode};//00,0x04
thef 1:b8d3e9e59703 79 char read_dist[1]={GET_Distance2BReg};//0x8f
akashvibhute 0:8e6304ab38d2 80 char read_vel[1]={GET_VelocityReg};
akashvibhute 0:8e6304ab38d2 81
akashvibhute 0:8e6304ab38d2 82 char dist[2];
akashvibhute 0:8e6304ab38d2 83 char vel[1];
akashvibhute 0:8e6304ab38d2 84
akashvibhute 0:8e6304ab38d2 85 nackack=1;
akashvibhute 0:8e6304ab38d2 86 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 87 {
akashvibhute 0:8e6304ab38d2 88 wait_ms(1);
akashvibhute 0:8e6304ab38d2 89 nackack = i2c_->write(LIDARLite_WriteAdr, write, 2);
akashvibhute 0:8e6304ab38d2 90 }
akashvibhute 0:8e6304ab38d2 91
akashvibhute 0:8e6304ab38d2 92 nackack=1;
akashvibhute 0:8e6304ab38d2 93 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 94 {
akashvibhute 0:8e6304ab38d2 95 wait_ms(1);
akashvibhute 0:8e6304ab38d2 96 nackack = i2c_->write(LIDARLite_WriteAdr, read_dist, 1);
akashvibhute 0:8e6304ab38d2 97 }
akashvibhute 0:8e6304ab38d2 98
akashvibhute 0:8e6304ab38d2 99 nackack=1;
akashvibhute 0:8e6304ab38d2 100 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 101 {
akashvibhute 0:8e6304ab38d2 102 wait_ms(1);
akashvibhute 0:8e6304ab38d2 103 nackack = i2c_->read(LIDARLite_ReadAdr, dist, 2);
akashvibhute 0:8e6304ab38d2 104 }
akashvibhute 0:8e6304ab38d2 105 distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1];
akashvibhute 0:8e6304ab38d2 106 }
akashvibhute 0:8e6304ab38d2 107 void LidarLite::refreshVelocity()
akashvibhute 0:8e6304ab38d2 108 {
akashvibhute 0:8e6304ab38d2 109 uint8_t nackack;
akashvibhute 0:8e6304ab38d2 110
akashvibhute 0:8e6304ab38d2 111 char write[2]={SET_CommandReg, AcqMode};
akashvibhute 0:8e6304ab38d2 112 char read_dist[1]={GET_Distance2BReg};
akashvibhute 0:8e6304ab38d2 113 char read_vel[1]={GET_VelocityReg};
akashvibhute 0:8e6304ab38d2 114
akashvibhute 0:8e6304ab38d2 115 char dist[2];
akashvibhute 0:8e6304ab38d2 116 char vel[1];
akashvibhute 0:8e6304ab38d2 117
akashvibhute 0:8e6304ab38d2 118 nackack=1;
akashvibhute 0:8e6304ab38d2 119 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 120 {
akashvibhute 0:8e6304ab38d2 121 wait_ms(1);
akashvibhute 0:8e6304ab38d2 122 nackack = i2c_->write(LIDARLite_WriteAdr, write, 2);
akashvibhute 0:8e6304ab38d2 123 }
akashvibhute 0:8e6304ab38d2 124
akashvibhute 0:8e6304ab38d2 125 nackack=1;
akashvibhute 0:8e6304ab38d2 126 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 127 {
akashvibhute 0:8e6304ab38d2 128 wait_ms(1);
akashvibhute 0:8e6304ab38d2 129 nackack = i2c_->write(LIDARLite_WriteAdr, read_vel, 1);
akashvibhute 0:8e6304ab38d2 130 }
akashvibhute 0:8e6304ab38d2 131
akashvibhute 0:8e6304ab38d2 132 nackack=1;
akashvibhute 0:8e6304ab38d2 133 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 134 {
akashvibhute 0:8e6304ab38d2 135 wait_ms(1);
akashvibhute 0:8e6304ab38d2 136 nackack = i2c_->read(LIDARLite_ReadAdr, vel, 1);
akashvibhute 0:8e6304ab38d2 137 }
akashvibhute 0:8e6304ab38d2 138 velocity_LL = (uint16_t)vel[0];
akashvibhute 0:8e6304ab38d2 139 }
thef 1:b8d3e9e59703 140 //For V2 sensor
thef 1:b8d3e9e59703 141
thef 1:b8d3e9e59703 142 void LidarLite::refreshVelocityAdd(char currentAddres)
thef 1:b8d3e9e59703 143 {
thef 1:b8d3e9e59703 144 uint8_t nackack;
thef 1:b8d3e9e59703 145
thef 1:b8d3e9e59703 146 char write[2]={SET_CommandReg, AcqMode};
thef 1:b8d3e9e59703 147 char read_dist[1]={GET_Distance2BReg};
thef 1:b8d3e9e59703 148 char read_vel[1]={GET_VelocityReg};
thef 1:b8d3e9e59703 149
thef 1:b8d3e9e59703 150 char dist[2];
thef 1:b8d3e9e59703 151 char vel[1];
thef 1:b8d3e9e59703 152
thef 1:b8d3e9e59703 153 nackack=1;
thef 1:b8d3e9e59703 154 while(nackack !=0)
thef 1:b8d3e9e59703 155 {
thef 1:b8d3e9e59703 156 wait_ms(1);
thef 1:b8d3e9e59703 157 nackack = i2c_->write((currentAddres<<1), write, 2);
thef 1:b8d3e9e59703 158 }
thef 1:b8d3e9e59703 159
thef 1:b8d3e9e59703 160 nackack=1;
thef 1:b8d3e9e59703 161 while(nackack !=0)
thef 1:b8d3e9e59703 162 {
thef 1:b8d3e9e59703 163 wait_ms(1);
thef 1:b8d3e9e59703 164 nackack = i2c_->write((currentAddres<<1), read_vel, 1);
thef 1:b8d3e9e59703 165 }
thef 1:b8d3e9e59703 166
thef 1:b8d3e9e59703 167 nackack=1;
thef 1:b8d3e9e59703 168 while(nackack !=0)
thef 1:b8d3e9e59703 169 {
thef 1:b8d3e9e59703 170 wait_ms(1);
thef 1:b8d3e9e59703 171 nackack = i2c_->read(((currentAddres<<1)|0x01), vel, 1);
thef 1:b8d3e9e59703 172 }
thef 1:b8d3e9e59703 173 velocity_LL = (uint16_t)vel[0];
thef 1:b8d3e9e59703 174 }
akashvibhute 0:8e6304ab38d2 175
akashvibhute 0:8e6304ab38d2 176 void LidarLite::refreshRangeVelocity()
akashvibhute 0:8e6304ab38d2 177 {
akashvibhute 0:8e6304ab38d2 178 uint8_t nackack;
akashvibhute 0:8e6304ab38d2 179
akashvibhute 0:8e6304ab38d2 180 char write[2]={SET_CommandReg, AcqMode};
akashvibhute 0:8e6304ab38d2 181 char read_dist[1]={GET_Distance2BReg};
akashvibhute 0:8e6304ab38d2 182 char read_vel[1]={GET_VelocityReg};
akashvibhute 0:8e6304ab38d2 183
akashvibhute 0:8e6304ab38d2 184 char dist[2];
akashvibhute 0:8e6304ab38d2 185 char vel[1];
akashvibhute 0:8e6304ab38d2 186
akashvibhute 0:8e6304ab38d2 187 nackack=1;
akashvibhute 0:8e6304ab38d2 188 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 189 {
akashvibhute 0:8e6304ab38d2 190 wait_ms(1);
akashvibhute 0:8e6304ab38d2 191 nackack = i2c_->write(LIDARLite_WriteAdr, write, 2);
akashvibhute 0:8e6304ab38d2 192 }
akashvibhute 0:8e6304ab38d2 193
akashvibhute 0:8e6304ab38d2 194 nackack=1;
akashvibhute 0:8e6304ab38d2 195 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 196 {
akashvibhute 0:8e6304ab38d2 197 wait_ms(1);
akashvibhute 0:8e6304ab38d2 198 nackack = i2c_->write(LIDARLite_WriteAdr, read_dist, 1);
akashvibhute 0:8e6304ab38d2 199 }
akashvibhute 0:8e6304ab38d2 200
akashvibhute 0:8e6304ab38d2 201 nackack=1;
akashvibhute 0:8e6304ab38d2 202 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 203 {
akashvibhute 0:8e6304ab38d2 204 wait_ms(1);
akashvibhute 0:8e6304ab38d2 205 nackack = i2c_->read(LIDARLite_ReadAdr, dist, 2);
akashvibhute 0:8e6304ab38d2 206 }
akashvibhute 0:8e6304ab38d2 207 distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1];
akashvibhute 0:8e6304ab38d2 208
akashvibhute 0:8e6304ab38d2 209
akashvibhute 0:8e6304ab38d2 210 nackack=1;
akashvibhute 0:8e6304ab38d2 211 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 212 {
akashvibhute 0:8e6304ab38d2 213 wait_ms(1);
akashvibhute 0:8e6304ab38d2 214 nackack = i2c_->write(LIDARLite_WriteAdr, read_vel, 1);
akashvibhute 0:8e6304ab38d2 215 }
akashvibhute 0:8e6304ab38d2 216
akashvibhute 0:8e6304ab38d2 217 nackack=1;
akashvibhute 0:8e6304ab38d2 218 while(nackack !=0)
akashvibhute 0:8e6304ab38d2 219 {
akashvibhute 0:8e6304ab38d2 220 wait_ms(1);
akashvibhute 0:8e6304ab38d2 221 nackack = i2c_->read(LIDARLite_ReadAdr, vel, 1);
akashvibhute 0:8e6304ab38d2 222 }
akashvibhute 0:8e6304ab38d2 223 velocity_LL = (uint16_t)vel[0];
thef 1:b8d3e9e59703 224 }
thef 1:b8d3e9e59703 225
thef 1:b8d3e9e59703 226 //For V2
thef 1:b8d3e9e59703 227 void LidarLite::refreshRangeVelocityAdd(char currentAddres)
thef 1:b8d3e9e59703 228 {
thef 1:b8d3e9e59703 229 uint8_t nackack;
thef 1:b8d3e9e59703 230
thef 1:b8d3e9e59703 231 char write[2]={SET_CommandReg, AcqMode};
thef 1:b8d3e9e59703 232 char read_dist[1]={GET_Distance2BReg};
thef 1:b8d3e9e59703 233 char read_vel[1]={GET_VelocityReg};
thef 1:b8d3e9e59703 234
thef 1:b8d3e9e59703 235 char dist[2];
thef 1:b8d3e9e59703 236 char vel[1];
thef 1:b8d3e9e59703 237
thef 1:b8d3e9e59703 238 nackack=1;
thef 1:b8d3e9e59703 239 while(nackack !=0)
thef 1:b8d3e9e59703 240 {
thef 1:b8d3e9e59703 241 wait_ms(1);
thef 1:b8d3e9e59703 242 nackack = i2c_->write((currentAddres<<1), write, 2);
thef 1:b8d3e9e59703 243 }
thef 1:b8d3e9e59703 244
thef 1:b8d3e9e59703 245 nackack=1;
thef 1:b8d3e9e59703 246 while(nackack !=0)
thef 1:b8d3e9e59703 247 {
thef 1:b8d3e9e59703 248 wait_ms(1);
thef 1:b8d3e9e59703 249 nackack = i2c_->write((currentAddres<<1), read_dist, 1);
thef 1:b8d3e9e59703 250 }
thef 1:b8d3e9e59703 251
thef 1:b8d3e9e59703 252 nackack=1;
thef 1:b8d3e9e59703 253 while(nackack !=0)
thef 1:b8d3e9e59703 254 {
thef 1:b8d3e9e59703 255 wait_ms(1);
thef 1:b8d3e9e59703 256 nackack = i2c_->read(((currentAddres<<1)|0x01), dist, 2);
thef 1:b8d3e9e59703 257 }
thef 1:b8d3e9e59703 258 distance_LL = ((uint16_t)dist[0] << 8) + (uint16_t)dist[1];
thef 1:b8d3e9e59703 259
thef 1:b8d3e9e59703 260
thef 1:b8d3e9e59703 261 nackack=1;
thef 1:b8d3e9e59703 262 while(nackack !=0)
thef 1:b8d3e9e59703 263 {
thef 1:b8d3e9e59703 264 wait_ms(1);
thef 1:b8d3e9e59703 265 nackack = i2c_->write((currentAddres<<1), read_vel, 1);
thef 1:b8d3e9e59703 266 }
thef 1:b8d3e9e59703 267
thef 1:b8d3e9e59703 268 nackack=1;
thef 1:b8d3e9e59703 269 while(nackack !=0)
thef 1:b8d3e9e59703 270 {
thef 1:b8d3e9e59703 271 wait_ms(1);
thef 1:b8d3e9e59703 272 nackack = i2c_->read(((currentAddres<<1)|0x01), vel, 1);
thef 1:b8d3e9e59703 273 }
thef 1:b8d3e9e59703 274 velocity_LL = (uint16_t)vel[0];
thef 1:b8d3e9e59703 275 }
thef 1:b8d3e9e59703 276
thef 1:b8d3e9e59703 277 void LidarLite::changeAddress(char currentAddres,char newAddress)
thef 1:b8d3e9e59703 278 {
thef 1:b8d3e9e59703 279
thef 1:b8d3e9e59703 280 char read_vel[1]={0x96};
thef 1:b8d3e9e59703 281 char write[2]={0x1a,};
thef 1:b8d3e9e59703 282 char write_sno[2]={0x18,};
thef 1:b8d3e9e59703 283 char write_sno1[2]={0x19,};
thef 1:b8d3e9e59703 284 char write_D[2]={0x1e,0x08}; //disable Primary Address
thef 1:b8d3e9e59703 285 write[1]= newAddress;
thef 1:b8d3e9e59703 286
thef 1:b8d3e9e59703 287 char s_no[2];
thef 1:b8d3e9e59703 288 uint8_t nackack=1;
thef 1:b8d3e9e59703 289
thef 1:b8d3e9e59703 290 while(nackack !=0)
thef 1:b8d3e9e59703 291 {
thef 1:b8d3e9e59703 292 wait_ms(1);
thef 1:b8d3e9e59703 293 nackack = i2c_->read(((currentAddres<<1)|0x01), s_no, 2);
thef 1:b8d3e9e59703 294 }
thef 1:b8d3e9e59703 295
thef 1:b8d3e9e59703 296
thef 1:b8d3e9e59703 297 write_sno[1]=s_no[0];
thef 1:b8d3e9e59703 298 write_sno1[1]=s_no[0];
thef 1:b8d3e9e59703 299
thef 1:b8d3e9e59703 300 nackack=1;
thef 1:b8d3e9e59703 301 while(nackack !=0)
thef 1:b8d3e9e59703 302 {
thef 1:b8d3e9e59703 303 wait_ms(1);
thef 1:b8d3e9e59703 304 i2c_->write((currentAddres<<1), write_sno, 2);//write 0x18
thef 1:b8d3e9e59703 305 }
thef 1:b8d3e9e59703 306
thef 1:b8d3e9e59703 307 nackack=1;
thef 1:b8d3e9e59703 308 while(nackack !=0)
thef 1:b8d3e9e59703 309 {
thef 1:b8d3e9e59703 310 wait_ms(1);
thef 1:b8d3e9e59703 311 i2c_->write((currentAddres<<1), write_sno1, 2);//write 0x18
thef 1:b8d3e9e59703 312 }
thef 1:b8d3e9e59703 313
thef 1:b8d3e9e59703 314 nackack=1;
thef 1:b8d3e9e59703 315 while(nackack !=0)
thef 1:b8d3e9e59703 316 {
thef 1:b8d3e9e59703 317 wait_ms(1);
thef 1:b8d3e9e59703 318 i2c_->write((currentAddres<<1), write, 2);
thef 1:b8d3e9e59703 319 }
thef 1:b8d3e9e59703 320
thef 1:b8d3e9e59703 321 nackack=1;
thef 1:b8d3e9e59703 322 while(nackack !=0)
thef 1:b8d3e9e59703 323 {
thef 1:b8d3e9e59703 324 wait_ms(1);
thef 1:b8d3e9e59703 325 i2c_->write((currentAddres<<1), write, 2);
thef 1:b8d3e9e59703 326 }
thef 1:b8d3e9e59703 327 nackack=1;
thef 1:b8d3e9e59703 328 while(nackack !=0)
thef 1:b8d3e9e59703 329 {
thef 1:b8d3e9e59703 330 wait_ms(1);
thef 1:b8d3e9e59703 331 i2c_->write((currentAddres<<1), write_D, 2);
thef 1:b8d3e9e59703 332 }
thef 1:b8d3e9e59703 333
akashvibhute 0:8e6304ab38d2 334 }