diff --git a/.gitignore b/.gitignore index f74c05b..fadb716 100644 --- a/.gitignore +++ b/.gitignore @@ -10,3 +10,6 @@ application.windows64 application.macosx out +.*~ +*.tsv +*.ods diff --git a/detection.pde b/detection.pde new file mode 100644 index 0000000..4460a18 --- /dev/null +++ b/detection.pde @@ -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 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); +} diff --git a/radar.pde b/radar.pde new file mode 100644 index 0000000..4a211ae --- /dev/null +++ b/radar.pde @@ -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 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;} +}