Initial commit
This commit is contained in:
parent
8ba24da704
commit
b897cf9734
|
|
@ -10,3 +10,6 @@ application.windows64
|
|||
application.macosx
|
||||
out
|
||||
|
||||
.*~
|
||||
*.tsv
|
||||
*.ods
|
||||
|
|
|
|||
|
|
@ -0,0 +1,32 @@
|
|||
class Detection {
|
||||
public int distance, x, y;
|
||||
public float speed, angle, magnitude, radAngle, distPx;
|
||||
public boolean freshData;
|
||||
|
||||
public Detection(byte[] data) {
|
||||
ByteBuffer bb = ByteBuffer.wrap(data);
|
||||
bb.order(ByteOrder.LITTLE_ENDIAN);
|
||||
distance = bb.getShort();
|
||||
speed = bb.getShort() / 100.0;
|
||||
angle = bb.getShort() / 100.0;
|
||||
magnitude = bb.getShort() / 100.0;
|
||||
radAngle = radians(angle);
|
||||
distPx = 500.0 / max_distance * distance;
|
||||
x = round(w_1_2 + sin(radAngle) * distPx);
|
||||
y = round(500 - abs(cos(radAngle)) * distPx);
|
||||
freshData = true;
|
||||
}
|
||||
|
||||
public String toString() {
|
||||
return String.format("%d\t%3.1f\t%5.2f\t%4.2f\t%5.2f\t%d\t%d", millis(), distance/100.0, speed, angle, magnitude, x, y);
|
||||
}
|
||||
}
|
||||
|
||||
public Detection[] parseDetectionData(byte[] data) {
|
||||
int sets = data.length / 8;
|
||||
Detection[] results = new Detection[sets];
|
||||
for(int i=0; i<sets; i++) {
|
||||
results[i] = new Detection(subset(data, i*8, 8));
|
||||
}
|
||||
return results;
|
||||
}
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
import java.nio.ByteBuffer;
|
||||
import java.nio.ByteOrder;
|
||||
|
||||
// Maximum distance displayed.
|
||||
int max_distance = 3000;
|
||||
|
||||
// The serial port used.
|
||||
String port = "/dev/tty.usbserial-00000000";
|
||||
|
||||
// Request method
|
||||
// PDAT returns all detected points, including reflections and noise
|
||||
// TDAT tries to track the strongest signal and returns only one point per request
|
||||
byte request_method = Radar.REQUEST_TYPE_TDAT;
|
||||
|
||||
int w_1_2 = 383;
|
||||
boolean waitingForData = false;
|
||||
boolean logSerial = false;
|
||||
int dataSets = 0;
|
||||
PrintWriter tsv;
|
||||
Radar radar;
|
||||
Detection[] detections;
|
||||
|
||||
void settings() {
|
||||
size(w_1_2*2, 500);
|
||||
}
|
||||
|
||||
void setup() {
|
||||
frameRate(10);
|
||||
radar = new Radar(this, port);
|
||||
radar
|
||||
.setMaximumSpeed(Radar.MAXIMUM_SPEED_100)
|
||||
.setMaximumRange(Radar.MAXIMUM_RANGE_30)
|
||||
.setThresholdOffset((byte)20)
|
||||
.setTrackingFilterType(Radar.TRACKING_FILTER_TYPE_FAST_DETECTION)
|
||||
.setMaximumDetectionDistance((byte)100)
|
||||
.setRangeThreshold((byte)0)
|
||||
.setSpeedThreshold((byte)100)
|
||||
.setHoldTime(1);
|
||||
|
||||
tsv = createWriter("log.tsv");
|
||||
|
||||
background(200);
|
||||
fill(255);
|
||||
distanceArc(max_distance, PIE);
|
||||
noFill();
|
||||
for (int i = 1000; i<max_distance; i+=1000) {
|
||||
distanceArc(i, OPEN);
|
||||
}
|
||||
tsv.println("timestamp\tdistance\tspeed\tangle\tmagnitude\tx\ty");
|
||||
System.out.println("timestamp\tdistance\tspeed\tangle\tmagnitude\tx\ty");
|
||||
}
|
||||
|
||||
void stop() {
|
||||
radar.stop();
|
||||
tsv.close();
|
||||
}
|
||||
|
||||
void draw() {
|
||||
fill(200);
|
||||
rect(0, 0, 20, 20);
|
||||
if (dataSets % 4 == 0) {
|
||||
circle(5, 5, 5);
|
||||
}
|
||||
if (dataSets % 4 == 1) {
|
||||
circle(10, 5, 5);
|
||||
}
|
||||
if (dataSets % 4 == 2) {
|
||||
circle(10, 10, 5);
|
||||
}
|
||||
if (dataSets % 4 == 3) {
|
||||
circle(5, 10, 5);
|
||||
}
|
||||
|
||||
radar.readResponse();
|
||||
drawData();
|
||||
|
||||
if (!waitingForData) {
|
||||
try { Thread.sleep(10); } catch(InterruptedException e) {}
|
||||
requestData();
|
||||
}
|
||||
}
|
||||
|
||||
void drawData() {
|
||||
if (detections!=null && detections.length > 0) { //<>//
|
||||
for (Detection d: detections) {
|
||||
if (d.freshData) {
|
||||
System.out.println(d.toString());
|
||||
tsv.println(d.toString());
|
||||
tsv.flush();
|
||||
drawPoint(d.x, d.y, d.speed, d.magnitude);
|
||||
d.freshData = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void distanceArc(int distance, int mode) {
|
||||
arc(w_1_2, 500, 500.0/max_distance*distance*2, 500.0/max_distance*distance*2, radians(-40 - 90), radians(40 - 90), mode);
|
||||
}
|
||||
|
||||
void requestData() {
|
||||
radar.requestDataFrame(request_method);
|
||||
waitingForData = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void drawPoint(int x, int y, float speed, float magnitude) {
|
||||
int dia;
|
||||
if (magnitude < 20) {
|
||||
noFill();
|
||||
dia = 2;
|
||||
} else {
|
||||
fill(220 - (magnitude-20)*10);
|
||||
dia = 15;
|
||||
}
|
||||
circle(x, y, dia);
|
||||
fill(0);
|
||||
textAlign(CENTER, BOTTOM);
|
||||
//text(speed, x, y-7);
|
||||
}
|
||||
|
|
@ -0,0 +1,136 @@
|
|||
import processing.serial.*;
|
||||
|
||||
class Radar {
|
||||
static final byte REQUEST_TYPE_PDAT = 0x04;
|
||||
static final byte REQUEST_TYPE_TDAT = 0x08;
|
||||
static final byte BASE_FREQUENCY_LOW = 0x00;
|
||||
static final byte BASE_FREQUENCY_MIDDLE = 0x01;
|
||||
static final byte BASE_FREQUENCY_HIGH = 0x02;
|
||||
static final byte MAXIMUM_SPEED_12_5 = 0x00;
|
||||
static final byte MAXIMUM_SPEED_25 = 0x01;
|
||||
static final byte MAXIMUM_SPEED_50 = 0x02;
|
||||
static final byte MAXIMUM_SPEED_100 = 0x03;
|
||||
static final byte MAXIMUM_RANGE_5 = 0x00;
|
||||
static final byte MAXIMUM_RANGE_10 = 0x01;
|
||||
static final byte MAXIMUM_RANGE_30 = 0x02;
|
||||
static final byte MAXIMUM_RANGE_100 = 0x03;
|
||||
static final byte TRACKING_FILTER_TYPE_STANDARD = 0x00;
|
||||
static final byte TRACKING_FILTER_TYPE_FAST_DETECTION = 0x01;
|
||||
static final byte TRACKING_FILTER_TYPE_LONG_VISIBILITY = 0x02;
|
||||
static final byte DETECTION_DIRECTION_RECEDING = 0x00;
|
||||
static final byte DETECTION_DIRECTION_APPROACHING = 0x01;
|
||||
static final byte DETECTION_DIRECTION_BOTH = 0x02;
|
||||
static final byte DIGITAL_OUTPUT_DIRECTION = 0x00;
|
||||
static final byte DIGITAL_OUTPUT_ANGLE = 0x01;
|
||||
static final byte DIGITAL_OUTPUT_RANGE = 0x02;
|
||||
static final byte DIGITAL_OUTPUT_SPEED = 0x03;
|
||||
static final byte DIGITAL_OUTPUT_MICRO_DETECTION = 0x04;
|
||||
static final byte RETRIGGER_OFF = 0x00;
|
||||
static final byte RETRIGGER_ON = 0x01;
|
||||
|
||||
Serial serial;
|
||||
public Radar(k_ld7 parent, String port) {
|
||||
serial = new Serial(parent, port, 115200, 'E', 8, 1.0);
|
||||
sendCommandAndWaitForResponse("INIT", new byte[]{0x00, 0x00, 0x00, 0x00});
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
serial.stop();
|
||||
}
|
||||
|
||||
private void sendCommand(String command, byte[] bytes) {
|
||||
if (logSerial) System.out.printf("Sending command %s...\n", command);
|
||||
serial.write(command);
|
||||
int len = (bytes == null) ? 0 : bytes.length;
|
||||
serial.write(new byte[] {(byte)(len & 0xFF), (byte)((len >> 8) & 0xFF), (byte)((len >> 16) & 0xFF), (byte)((len >> 24) & 0xFF)});
|
||||
if (len > 0) serial.write(bytes);
|
||||
}
|
||||
|
||||
private void sendCommandAndWaitForResponse(String command, byte[] bytes) {
|
||||
sendCommand(command, bytes);
|
||||
while(!readResponse().equals("RESP")) {try{Thread.sleep(100);}catch(Exception e){}};
|
||||
}
|
||||
|
||||
private byte[] readBytes(int count) {
|
||||
byte[] result = serial.readBytes(count);
|
||||
if (logSerial) {
|
||||
for (int i=0; i<count; i++) {
|
||||
System.out.printf("0x%02X ", result[i]);
|
||||
}
|
||||
System.out.println();
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
public String readResponse() {
|
||||
if (logSerial) System.out.printf("readResponse(): %d bytes available\n", serial.available());
|
||||
if (serial.available() < 8) {
|
||||
return "";
|
||||
}
|
||||
String command = new String(readBytes(4));
|
||||
byte[] l = readBytes(4);
|
||||
int len = l[0] + (l[1]<<8) + (l[2]<<16) + (l[3]<<24);
|
||||
if (logSerial) System.out.printf("Receiving %s with %d Bytes payload...", command, len);
|
||||
byte[] payload = readBytes(len);
|
||||
if (logSerial) System.out.println(" complete.");
|
||||
|
||||
if (command.equals("RESP") && len==1) {
|
||||
switch (payload[0]) {
|
||||
case 0: break; // OK
|
||||
case 1: System.out.println("Unknown command"); break;
|
||||
case 2: System.out.println("Invalid parameter value"); break;
|
||||
case 3: System.out.println("Invalid RPST version"); break;
|
||||
case 4: System.out.println("Uart error (parity, framing, noise)"); break;
|
||||
case 5: System.out.println("Sensor busy"); break;
|
||||
case 6: System.out.println("Timeout error"); break;
|
||||
default: System.out.printf("Unknown error code %d\n", payload[0]); break;
|
||||
}
|
||||
} else if (command.equals("PDAT")) {
|
||||
if (len > 0) {
|
||||
detections = parseDetectionData(payload);
|
||||
}
|
||||
waitingForData = false;
|
||||
dataSets++;
|
||||
} else if (command.equals("TDAT")) {
|
||||
if (len > 0) {
|
||||
detections = parseDetectionData(payload);
|
||||
}
|
||||
waitingForData = false;
|
||||
dataSets++;
|
||||
} else if (command.equals("RPST")) {
|
||||
for(byte b : payload) {
|
||||
System.out.printf("%02X ", b);
|
||||
}
|
||||
System.out.println();
|
||||
}
|
||||
|
||||
return command;
|
||||
}
|
||||
|
||||
public void requestDataFrame(byte type) {
|
||||
sendCommand("GNFD", new byte[] {type, 0, 0, 0});
|
||||
}
|
||||
|
||||
public Radar setBaseFrequency(byte type) { sendCommandAndWaitForResponse("RBFR", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setMaximumSpeed(byte type) { sendCommandAndWaitForResponse("RSPI", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setMaximumRange(byte type) { sendCommandAndWaitForResponse("RRAI", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setThresholdOffset(byte value) { sendCommandAndWaitForResponse("THOF", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setTrackingFilterType(byte type) { sendCommandAndWaitForResponse("TRFT", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setVibrationSuppression(byte value) { sendCommandAndWaitForResponse("VISU", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setMinimumDetectionDistance(byte type) { sendCommandAndWaitForResponse("MIRA", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setMaximumDetectionDistance(byte type) { sendCommandAndWaitForResponse("MARA", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setMinimumDetectionAngle(byte value) { sendCommandAndWaitForResponse("MIAN", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setMaximumDetectionAngle(byte value) { sendCommandAndWaitForResponse("MAAN", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setMinimumDetectionSpeed(byte value) { sendCommandAndWaitForResponse("MISP", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setMaximumDetectionSpeed(byte value) { sendCommandAndWaitForResponse("MASP", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setDetectionDirection(byte type) { sendCommandAndWaitForResponse("DEDI", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setRangeThreshold(byte value) { sendCommandAndWaitForResponse("RATH", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setAngleThreshold(byte value) { sendCommandAndWaitForResponse("ANTH", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setSpeedThreshold(byte value) { sendCommandAndWaitForResponse("SPTH", new byte[]{value, 0, 0, 0}); return this;}
|
||||
public Radar setDigitalOutput1(byte type) { sendCommandAndWaitForResponse("DIG1", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setDigitalOutput2(byte type) { sendCommandAndWaitForResponse("DIG2", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setDigitalOutput3(byte type) { sendCommandAndWaitForResponse("DIG3", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setHoldTime(int value) { sendCommandAndWaitForResponse("DIG1", new byte[]{(byte)(value & 0xFF), (byte)((value >> 8) & 0xFF), 0, 0}); return this;}
|
||||
public Radar setMicroDetectionRetrigger(byte type) { sendCommandAndWaitForResponse("MIDE", new byte[]{type, 0, 0, 0}); return this;}
|
||||
public Radar setMicroDetectionSensitivity(byte type) { sendCommandAndWaitForResponse("MIDS", new byte[]{type, 0, 0, 0}); return this;}
|
||||
}
|
||||
Loading…
Reference in New Issue