I2C Write / LIDAR Lite not working

Technical C/C++
1

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;
}
2