I2C Write / LIDAR Lite not working

[GGeorge]

I recently ordered a LIDAR Lite (http://pulsedlight3d.com/products/lidar-lite), and it hasn't worked for longer than about 1 second each time I run my program.

I'm using the Write function in WPILib's built in I2C class. I believe that the LIDAR requires that the RoboRio writes 0x04 to I2C register 0x00 every once in a while. However, the Write function is returning true, which indicates that the write didn't work. There are no issues with reading though.

One lead I got was that reading register 0x01 usually returns "sig overflow flag" after the sensor stops working.

Can anyone offer insight on this issue?

Here's my code below


// LIDAR.h

#ifndef SRC_LIDAR_H_
#define SRC_LIDAR_H_

#include "WPILib.h"

class LIDAR: public I2C {
public:
LIDAR();
virtual ~LIDAR();
int getCM();
int poll();
void setup();
unsigned char getHigh();
unsigned char getLow();
unsigned char getLidarStatus();

private:

unsigned char distanceArray[2];
unsigned char *lidarStatus;
int cmReading;
int counter;
};

#endif /* SRC_LIDAR_H_ */




// LIDAR.cpp

#include <LIDAR.h>

#define address 0x62
#define controlAddress 0x00
#define statusAddress 0x01
#define measureValue 0x04
#define registerHigh 0x0f
#define registerLow 0x10
#define registerHL 0x8f

LIDAR::LIDAR() : I2C(I2C::Port::kMXP, address), counter(0)
{
distanceArray[0]='0';
distanceArray[1]='0';
lidarStatus = new unsigned char();
}

LIDAR::~LIDAR() {
}

int LIDAR::getCM(){
return cmReading;
}

void LIDAR::setup(){
I2C::Write(controlAddress, 0x00);
}

int LIDAR::poll(){

//It's a bit rudimentary, I know

int status=0;
if (I2C::Write(0x04, 0x00)){
status+=1;
}
if (I2C::Write(controlAddress, measureValue)){
status+=2;
}
if (I2C::Read(registerHL, 2, distanceArray)){
status+=4;
}
if (I2C::Read(statusAddress, 1, lidarStatus)){
status+=8;
}
if (I2C::Write(0x04, 0x00)){
status+=16;
}
cmReading=(int)((distanceArray[0] <<8) + distanceArray[1]);
return status;
}

unsigned char LIDAR::getHigh(){
return distanceArray[0];
}

unsigned char LIDAR::getLow(){
return distanceArray[1];
}

unsigned char LIDAR::getLidarStatus(){
return *lidarStatus;
}