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);
//requires(Robot.lidar);
}
// 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) {
case REQUEST_NEW_DISTANCE :
i2c.write(LIDAR_CONFIG_REGISTER, 0x04);
System.out.println("REQUEST_NEW_DISTANCE");
lidarState = LidarState.DELAY;
break;
case DELAY :
System.out.println("DELAY");
lidarState = LidarState.PULL_FOR_NEW_DATA;
break;
case PULL_FOR_NEW_DATA :
System.out.println("PULL_FOR_NEW_DATA");
i2c.read(LIDAR_STATUS_REGISTER, 1, status);
if((status[0] & 0x01) == 0) {
lidarState = LidarState.READ_NEW_DISTANCE;
}
break;
case READ_NEW_DISTANCE :
i2c.read(LIDAR_DISTANCE_REGISTER, 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]);
//Robot.lidar.setDistanceInches(actualDistance);
System.out.println("LIDAR Distance In Inches: " + actualDistance);
lidarState = LidarState.REQUEST_NEW_DISTANCE;
break;
}
}
// 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() {
end();
}
private enum LidarState {
DELAY, REQUEST_NEW_DISTANCE, PULL_FOR_NEW_DATA, READ_NEW_DISTANCE
}
}
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.