So, we have been having issues with the TFMini not registering values right and can’t seem to figure it out. We’ve checked over the wiring multiple times, so it seems the code is the problem but we just can’t seem to see where the problem is. Anyone got some fixes or or advice?
Here is our code:
package frc.robot.subsystems;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class Lidar extends SerialPort implements Runnable
{
private static int dataBit = 8;
private static int stopBit = 1;
private static int count = 0;
private byte[] packet = new byte[9];
public Lidar()
{
super(115200, Port.kMXP, dataBit, Parity.kNone, StopBits.kOne);
//Baud Rate, Port #, Data Bit, Parity, Stop Bit
setReadBufferSize(9);
}
private short combine(byte one, byte two)
{
return (short) ((one & 0xFF) | (two << 8));
}
@Override
public void run()
{
while(true!Thread.interrupted()
{
packet = read(9);
SmartDashboard.putNumber("readable",getBytesReceived());
SmartDashboard.putNumber("length",packet.length);
SmartDashboard.putNumber("check", count);
count++;
SmartDashboard.putNumber("Lidar distance", combine(packet[2], packet[3]));
for (int x = 0; x < packet.length - 1; x++)
{
if (packet[x] == 0x59 && packet[x + 1] == 0x59)
{
if (x < 5)
{
SmartDashboard.putBoolean("pased check", true);
SmartDashboard.putNumber("Lidar distance", combine(packet[x + 2], packet[x + 3]));
SmartDashboard.putString("Lidar one", Integer.toHexString(packet[x+2]));
SmartDashboard.putString("Lidar two", Integer.toHexString(packet[x+3]));
}
else
SmartDashboard.putBoolean("pased check", false);
}
}
}
}
}