Skip to content

Commit

Permalink
Merge branch 'dev' of https://github.com/letsbuildrockets/ToF-CAN int…
Browse files Browse the repository at this point in the history
…o dev
  • Loading branch information
ericsims committed Apr 7, 2019
2 parents 80be743 + aee14c4 commit 53ba185
Show file tree
Hide file tree
Showing 5 changed files with 69 additions and 23 deletions.
Binary file removed software/ToF-CAN-Java-FRC-V1.0.zip
Binary file not shown.
Binary file added software/ToF-CAN-Java-FRC-V1.1.zip
Binary file not shown.
16 changes: 11 additions & 5 deletions software/java_frc/exampleRobot.java
Original file line number Diff line number Diff line change
@@ -1,19 +1,25 @@
import org.letsbuildrocekts.libs.TimeOfFlightSensor;
package frc.robot;

import org.letsbuildrockets.libs.TimeOfFlightSensor;

import edu.wpi.first.wpilibj.TimedRobot;

public class Robot extends TimedRobot {
private TimeOfFlightSensor tofsensor;

@Override
public void robotInit() {
tofsensor = new TimeOfFlightSensor(0x620);
tofsensor = new TimeOfFlightSensor(0x621);
System.out.println("ToF Sensor at " + String.format("0x%03x", tofsensor.getID()) + "! (with firmware version: " + tofsensor.getFirwareVersion().toString()+")");
}

@Override
public void teleopPeriodic() {
if(tofsensor.inRange())
System.out.println("distance: " + tofsensor.getDistance());
else
if(tofsensor.inRange()) {
System.out.println("distance: " + tofsensor.getDistance()+ " " + tofsensor.getError());
} else {
System.out.println("out of range");
}
}

}
68 changes: 50 additions & 18 deletions software/java_frc/org/letsbuildrockets/libs/TimeOfFlightSensor.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,13 +2,16 @@

import org.letsbuildrockets.libs.CustomCAN;
import org.letsbuildrockets.libs.VersionNumber;

import edu.wpi.first.hal.util.UncleanStatusException;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.hal.HAL;
/**
* TimeOfFlightSensor
* V1.1
*/
public class TimeOfFlightSensor {


// Control Bytes
private static final byte CTRL_SEND_ERROR = 1;
private static final byte CTRL_SEND_DISTANCE = 2;
Expand All @@ -25,61 +28,77 @@ public class TimeOfFlightSensor {
private static final byte ERROR_BAD_CTRL_BYTE = 5;
private static final byte ERROR_NOT_ENOUGH_DATA_BYTES = 6;

// Required Firmware
private static final VersionNumber minVersion = new VersionNumber(1,1);

int _ID, _distance, _error;
VersionNumber _firmwareVersion;
CustomCAN tofsensor;
static int TOFCount = 0;

protected int _ID, _distance, _error = -1;
protected Timer packetTimer;
protected VersionNumber _firmwareVersion;
protected CustomCAN tofsensor;
protected static int TOFCount = 0;

public TimeOfFlightSensor(int ID) {
tofsensor = new CustomCAN("TOF"+String.valueOf(TOFCount), ID);
_firmwareVersion = new VersionNumber(0, 0);
_ID = ID;
TOFCount++;
sendByte(CTRL_GET_FIRMWARE_VERSION);
readBuffer();
packetTimer = new Timer();
packetTimer.start();
getFirwareVersion();
if(_firmwareVersion.isOlderThan(minVersion))
HAL.sendError(true, -2, false, "LBR: Old Firmware! ToF sensor at " + String.format("0x%04x", _ID) + " is on firmware version " + _firmwareVersion.toString() + " but version " + minVersion.toString() + " is required. Upgrade ToF firmware, or downagrade the TimeOfFlight Java library!", "", "", false);
}

private void readBuffer() {
try {
byte dat[] = tofsensor.readSafely();
// for (byte byteme : dat) {
// System.out.printf("rec: 0x%02x\n", byteme);
// System.out.printf("rec: 0x%02x\n", byteme);
// }
switch (dat[0]) {
case CTRL_SEND_ERROR:
if(dat.length == 2){
_error = dat[1];
if(_error == ERROR_NONE || _error == ERROR_OUT_OF_RANGE)
packetTimer.reset();
}
break;
case CTRL_SEND_DISTANCE:
if(dat.length == 3) {
_error = dat[0] & 0xFF;
if(dat.length == 4) {
_distance = (dat[1]&0xFF) << 8 | (dat[2]&0xFF);
// System.out.println("distance: " + _distance);
_error = dat[3];
//System.out.println("distance: " + _distance);
packetTimer.reset();
}
break;
case CTRL_SEND_FIRMWARE_VERSION:
//System.out.println("ok, we received a firmware number i think");
if(dat.length == 3){
_firmwareVersion.major = dat[1];
_firmwareVersion.minor = dat[2];
_firmwareVersion = new VersionNumber(dat[1], dat[2]);
}
packetTimer.reset();
break;

default:
break;
}
} catch (CANMessageUnavailableException e) {
//System.out.println("CAN error "+e.getMessage());
if(packetTimer.hasPeriodPassed(5))
HAL.sendError(true, -1, false, "LBR: Unable to communicate with ToF sensor at " + String.format("0x%04x", _ID), "", e.getStackTrace().toString(), false);
}
}

private void sendByte(byte byteme) {
byte dat[] = new byte[8];
dat[0] = byteme;
try {
tofsensor.writeSafely(dat);
} catch (UncleanStatusException e) { }
}

public void setHarwareCANAddress(short newID) {
public void setHarwareCANAddress(int newID) {
if(newID < 0x0620) {
System.err.println("The new address for " + tofsensor.getName() + " must be >= 0x0620!");
return;
Expand All @@ -96,23 +115,36 @@ public void setHarwareCANAddress(short newID) {
}

public VersionNumber getFirwareVersion() {
if(_firmwareVersion.major > 0) return _firmwareVersion;
sendByte(CTRL_GET_FIRMWARE_VERSION);
readBuffer();
for(int i = 0; i < 100; i++) {
Timer.delay(0.01);
readBuffer();
if(_firmwareVersion.major > 0) return _firmwareVersion;
}
HAL.sendError(true, -1, false, "LBR: Unable to communicate with ToF sensor at " + String.format("0x%04x", _ID) + " Can't get firmware version!", "", "", false);
return _firmwareVersion;
}

public int getID() {
return _ID;
}

public int getDistance() {
readBuffer();
if(packetTimer.hasPeriodPassed(0.01))
readBuffer();
return _distance;
}

public int getError() {
readBuffer();
if(packetTimer.hasPeriodPassed(0.01))
readBuffer();
return _error;
}

public boolean inRange() {
readBuffer();
if(packetTimer.hasPeriodPassed(0.01))
readBuffer();
return (_error == 0);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,4 +11,12 @@ public class VersionNumber {
major = _major;
minor = _minor;
}

public boolean isOlderThan(VersionNumber req) {
return(major < req.major || (major == req.major && minor < req.minor));
}

public String toString() {
return String.format("%d.%d", major, minor);
}
}

0 comments on commit 53ba185

Please sign in to comment.