The team has been trying to get a new LIDAR Lite 3 unit to work for the past couple of days. We’ve found five or six examples that for the most part do approximately the same thing. Most start another thread. I think 254 did a wait or delay statement in the current thread.

We went at it as a single command and used a state machine in execute to go through the process. Basically we write the request for a new distance, using the state machine and a delay state and the scan rate of the RR ultimately wait 40ms until we poll the is busy register. Once that indicates it isn’t busy we read the new distance.

The problem seems to be that our distance values never change. Not just our distance after the cm to inch conversion, but the original two byte array.

We have also used the i2c port addressonly(?) method to see if the device is on the network. We always get a response of true. I believe this indicates that the poll to that address failed. Initially we tried 0x62 which I think is the default address. For grins we put in a for loop and tried 0x00 0xff just to see if we’d get an answer somewhere other than expected. Nothing.

Here’s our code:

package org.usfirst.frc3098.RobotCode2017.commands;

import org.usfirst.frc3098.RobotCode2017.Robot;

import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.I2C.Port;
import edu.wpi.first.wpilibj.command.Command;

public class CmdLIDARGetDistance extends Command {
	private I2C i2c;
    private byte] distance;
    private byte] status;
    private LidarState lidarState;
    private double actualDistance;
    private int LIDAR_ADDR = 0x62;
    private final int LIDAR_CONFIG_REGISTER = 0x00;
    private final int LIDAR_DISTANCE_REGISTER = 0x0f; 
    private final int LIDAR_STATUS_REGISTER = 0x01;

    public CmdLIDARGetDistance() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);

    // Called just before this Command runs the first time
    protected void initialize() {
    	i2c = new I2C(Port.kOnboard, LIDAR_ADDR);
        distance = new byte[2];
        status = new byte[1];
        lidarState = LidarState.REQUEST_NEW_DISTANCE;

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	switch(lidarState) {
    			i2c.write(LIDAR_CONFIG_REGISTER, 0x04);
    			lidarState = LidarState.DELAY;
    		case DELAY :
    			lidarState = LidarState.PULL_FOR_NEW_DATA;
    		case PULL_FOR_NEW_DATA :
    			System.out.println("PULL_FOR_NEW_DATA");, 1, status);
    			if((status[0] & 0x01) == 0) {
    				lidarState = LidarState.READ_NEW_DISTANCE;
    		case READ_NEW_DISTANCE :, 2, distance);
    			actualDistance = (int)(Integer.toUnsignedLong(distance[0] << 8) 
    									+ Byte.toUnsignedInt(distance[1]) * 0.393701);
    			System.out.println("distance 1 " + distance[0]);
    			System.out.println("distance 2 " + distance[1]);
    			System.out.println("LIDAR Distance In Inches: " + actualDistance);
    			lidarState = LidarState.REQUEST_NEW_DISTANCE;

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;

    // Called once after isFinished returns true
    protected void end() {

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    private enum LidarState {

Can anyone see something wrong with this? This is a brand new unit out of the box. We put it on another robot that was using an arduino and they got an answer out of it so we believe the unit is ok. We have also tried both the onboard port and the mxp port. No luck on either of them. We’ve also tried exact copies of some of the other code (multithreaded examples) with no luck.

Any thoughts?

Thanks ahead.

Try switching your lidar config distance value to 0x8F from 0x0F. Thats what ours is set to and works fine.

If that still doesnt work, you can take a look at our code if you want to compare. We also use a separate thread for the lidar like the other examples you’ve seen, but it’s completely abstracted from you.

Lastly, don’t use the i2c port on the rio if you’re using java. We have never been able to get it to work with our lidar. We did, however, get it to work with the mxp port.

Thanks for the info. I’d like to try your code. Is it on guit hub?

We have tried it on both the onboard port and the mxp port multiple times with all of the variations we have tried.

I’m a little confused about the 0x8f register. Yes, we’ve seen this all over the place. But the manual shows specifically 0x0f and 0x10 for the distance. That’s two registers in a row that would fit the high byte, low byte order that has been described. We have tried various code examples with both 0x8f and 0x0f for the address. Ahh way we’ve tried this has been a failure so far.

Sorry I forgot to link it