|
@@ -7,6 +7,7 @@ import java.io.DataOutputStream;
|
|
import java.io.IOException;
|
|
import java.io.IOException;
|
|
import java.net.Socket;
|
|
import java.net.Socket;
|
|
import java.nio.ByteBuffer;
|
|
import java.nio.ByteBuffer;
|
|
|
|
+import java.nio.ByteOrder;
|
|
import java.util.ArrayList;
|
|
import java.util.ArrayList;
|
|
import java.util.List;
|
|
import java.util.List;
|
|
|
|
|
|
@@ -24,34 +25,33 @@ public class RTDEClient implements Runnable {
|
|
this.port=port;
|
|
this.port=port;
|
|
}
|
|
}
|
|
|
|
|
|
- public void start() {
|
|
|
|
- Thread readThread = new Thread(this);
|
|
|
|
- readThread.start();
|
|
|
|
- send_output_setup(List.of(OutParams.speed_scaling),List.of(),125);
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- // Internal method that actually reads the data
|
|
|
|
- private void readSocket() throws IOException {
|
|
|
|
- try(Socket rt = new Socket(ip, port);){
|
|
|
|
|
|
+ public void start() throws IOException {
|
|
|
|
+ try (Socket rt = new Socket(ip, port);) {
|
|
rt.setSoTimeout(0);
|
|
rt.setSoTimeout(0);
|
|
- if (rt.isConnected()){
|
|
|
|
|
|
+ if (rt.isConnected()) {
|
|
System.out.println("Connected to UR Realtime Client");
|
|
System.out.println("Connected to UR Realtime Client");
|
|
}
|
|
}
|
|
dis = new DataInputStream(rt.getInputStream());
|
|
dis = new DataInputStream(rt.getInputStream());
|
|
dos = new DataOutputStream(rt.getOutputStream());
|
|
dos = new DataOutputStream(rt.getOutputStream());
|
|
|
|
+ Thread readThread = new Thread(this);
|
|
|
|
+ readThread.start();
|
|
|
|
+ send_output_setup(List.of(OutParams.speed_scaling), List.of(), 125);
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
|
|
-
|
|
|
|
- while(true) {
|
|
|
|
- int length = dis.readInt();
|
|
|
|
- double[] rtm = new double[length];
|
|
|
|
- rtm[0] = length;
|
|
|
|
- // Calculate how much data is available from the length
|
|
|
|
- int data_available = (length - 4) / 8;
|
|
|
|
- for(int i=0; i<data_available; i++){
|
|
|
|
- rtm[i] = dis.readDouble();
|
|
|
|
- }
|
|
|
|
|
|
+ // Internal method that actually reads the data
|
|
|
|
+ private void readSocket() throws IOException {
|
|
|
|
+ while(true) {
|
|
|
|
+ int length = dis.readInt();
|
|
|
|
+ double[] rtm = new double[length];
|
|
|
|
+ rtm[0] = length;
|
|
|
|
+ // Calculate how much data is available from the length
|
|
|
|
+ int data_available = (length - 4) / 8;
|
|
|
|
+ for(int i=0; i<data_available; i++){
|
|
|
|
+ rtm[i] = dis.readDouble();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
+
|
|
}
|
|
}
|
|
|
|
|
|
@Override
|
|
@Override
|
|
@@ -77,21 +77,28 @@ public class RTDEClient implements Runnable {
|
|
|
|
|
|
|
|
|
|
public void send_output_setup(List<String> variables, List<String> types, int frequency) {
|
|
public void send_output_setup(List<String> variables, List<String> types, int frequency) {
|
|
- Bytes<ByteBuffer> bytes = Bytes.elasticHeapByteBuffer(64);
|
|
|
|
- bytes.writeDouble(frequency);
|
|
|
|
var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
|
|
var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
|
|
var payload = String.join(",", variables);
|
|
var payload = String.join(",", variables);
|
|
- sendAndReceive(cmd, payload.getBytes());
|
|
|
|
|
|
+ byte[] p= payload.getBytes();
|
|
|
|
+ ByteBuffer bytes = ByteBuffer.allocate(8+p.length);
|
|
|
|
+ bytes.putDouble(frequency);
|
|
|
|
+ bytes.put(p);
|
|
|
|
+ sendAndReceive(cmd, bytes.array());
|
|
}
|
|
}
|
|
|
|
|
|
public void sendall(CommandType cmd, byte[] payload) {
|
|
public void sendall(CommandType cmd, byte[] payload) {
|
|
try {
|
|
try {
|
|
- Bytes<ByteBuffer> bytes = Bytes.elasticHeapByteBuffer(64);
|
|
|
|
int size = 3 + payload.length;
|
|
int size = 3 + payload.length;
|
|
- bytes.writeUnsignedShort(size);
|
|
|
|
- bytes.writeUnsignedByte(cmd.getVal());
|
|
|
|
- bytes.write(payload);
|
|
|
|
- dos.write(bytes.toByteArray());
|
|
|
|
|
|
+ ByteBuffer bytes = ByteBuffer.allocate(size);
|
|
|
|
+ bytes.order(ByteOrder.BIG_ENDIAN);
|
|
|
|
+ bytes.order( ByteOrder.BIG_ENDIAN);
|
|
|
|
+ bytes.putShort((short) size);
|
|
|
|
+ bytes.put((byte) cmd.getVal());
|
|
|
|
+ bytes.put(payload);
|
|
|
|
+ bytes.rewind();
|
|
|
|
+ byte[] arr = new byte[bytes.remaining()];
|
|
|
|
+ bytes.get(arr);
|
|
|
|
+ dos.write(arr);
|
|
dos.flush();
|
|
dos.flush();
|
|
} catch (IOException e) {
|
|
} catch (IOException e) {
|
|
e.printStackTrace();
|
|
e.printStackTrace();
|