Martin Kunz 2 years ago
parent
commit
f954019884

+ 5 - 0
pom.xml

@@ -69,6 +69,11 @@
 	</build>
 
 	<dependencies>
+		<dependency>
+			<groupId>net.openhft</groupId>
+			<artifactId>chronicle-bytes</artifactId>
+			<version>2.19.0</version>
+		</dependency>
 		<dependency>
 			<groupId>com.nmote.xr</groupId>
 			<artifactId>nmote-xr</artifactId>

+ 9 - 0
src/main/java/at/acdp/urweb/Main.java

@@ -1,8 +1,10 @@
 package at.acdp.urweb;
 
+import at.acdp.urweb.fhpp.FHMaster;
 import at.acdp.urweb.fhpp.FHMasterSim;
 import at.acdp.urweb.fhpp.FHPP;
 import at.acdp.urweb.fhpp.IFHMaster;
+import at.acdp.urweb.rtde.RTDEClient;
 import at.acdp.urweb.web.FestoXRServer;
 import com.nmote.nanohttp.NanoServer;
 import com.nmote.xr.HTTPServerEndpoint;
@@ -16,7 +18,14 @@ public class Main {
         Params app = null;
         try {
             app = picocli.CommandLine.populateCommand(new Params(), args);
+            if(app==null) {
+                System.exit(1);
+            }
             NanoServer server = new NanoServer("http://0.0.0.0:" + app.port);
+
+            RTDEClient r=new RTDEClient("127.0.0.1", 30004);
+            r.start();
+
             FestoXRServer fxrs = new FestoXRServer();
             HTTPServerEndpoint xrs = XR.server(fxrs, FestoXRServer.class);
             server.add(xrs);

+ 25 - 10
src/main/java/at/acdp/urweb/fhpp/FHMaster.java

@@ -120,7 +120,7 @@ public class FHMaster implements IFHMaster {
     public CompletableFuture<Status> readStatus() {
         if(master==null)
             return null;
-        CompletableFuture<ReadHoldingRegistersResponse> f =master.sendRequest(new ReadHoldingRegistersRequest(0,4), 0);
+        CompletableFuture<ReadHoldingRegistersResponse> f=master.sendRequest(new ReadHoldingRegistersRequest(0,4), 0);
 
         CompletableFuture<Status> fc= f.thenApply((response) -> {
             if (response != null) {
@@ -131,8 +131,6 @@ public class FHMaster implements IFHMaster {
                     Status s=new Status();
                     s.read(bytes);
                     return s;
-                    // System.out.println(s.toString());
-                    // System.out.println(ByteBufUtil.hexDump(bb));
                 }
                 catch (Exception e) {
                     e.printStackTrace();
@@ -147,19 +145,36 @@ public class FHMaster implements IFHMaster {
         return  fc;
     }
 
-    @Override
-    public void bereitschaft() {
-        List<ModbusRequest> blist = new ArrayList<>();
-        blist.add(new WriteMultipleRegistersRequest(0,4, new byte[]{0b01000111,0,0,0,0,0,0,0}));
+    private void trySleep(int ms) {
         try {
-            Thread.sleep(10);
-            sendRequests(master, blist);
-            Thread.sleep(10);
+            Thread.sleep(ms);
         } catch (InterruptedException e) {
             e.printStackTrace();
         }
     }
 
+    private void sendReq(byte[] what) {
+        List<ModbusRequest> blist = new ArrayList<>();
+        blist.add(new WriteMultipleRegistersRequest(0,4, what));
+        trySleep(10);
+        sendRequests(master, blist);
+        trySleep(10);
+
+    }
+
+    @Override
+    public void bereitschaft() {
+
+        //S. 135 "Betriebsbereitschaft
+        sendReq(new byte[]{0b00000001,0,0,0,0,0,0,0});
+        sendReq(new byte[]{0b00000011,0,0,0,0,0,0,0});
+
+
+        //S. 140 Betriebsbereischaft - direktauftrag
+       // sendReq(new byte[]{0b00000100,0,0,0,0,0,0,0});
+       // sendReq(new byte[]{0b01000111,0,0,0,0,0,0,0});
+    }
+
     @Override
     public void stop() {
         started = false;

+ 6 - 3
src/main/java/at/acdp/urweb/fhpp/Status.java

@@ -4,7 +4,8 @@ import java.util.StringJoiner;
 
 public class Status {
     //SCON
-    public int opm;
+    public boolean opm1;
+    public boolean opm2;
     public boolean fct;
     public boolean rdyen;
     public boolean fault;
@@ -34,7 +35,8 @@ public class Status {
     @Override
     public String toString() {
         return new StringJoiner("<br>", Status.class.getSimpleName() + "[", "]")
-                .add("opm=" + opm)
+                .add("opm1=" + opm1)
+                .add("opm2=" + opm2)
                 .add("fct=" + fct)
                 .add("rdyen=" + rdyen)
                 .add("fault=" + fault)
@@ -69,7 +71,8 @@ public class Status {
 
     public void read(byte[] status) {
         byte scon=status[0];
-        opm =  (scon & (1 <<7)  | scon & (1 <<6));
+        opm1 = (scon & (1 << 7))>0;
+        opm2 = (scon & (1 << 6))>0;
         fct = (scon & (1 << 5))>0;
         rdyen = (scon & (1 << 4))>0;
         fault = (scon & (1 << 3))>0;

+ 9 - 0
src/main/java/at/acdp/urweb/rtde/Command.java

@@ -0,0 +1,9 @@
+package at.acdp.urweb.rtde;
+
+import java.util.List;
+
+public class Command {
+    CommandType type;
+    List<String> variables;
+
+}

+ 17 - 0
src/main/java/at/acdp/urweb/rtde/CommandType.java

@@ -0,0 +1,17 @@
+package at.acdp.urweb.rtde;
+
+public enum CommandType {
+    RTDE_REQUEST_PROTOCOL_VERSION(86),
+    RTDE_GET_URCONTROL_VERSION(118),
+    RTDE_TEXT_MESSAGE(77),
+    RTDE_DATA_PACKAGE(85),
+    RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS(79),
+    RTDE_CONTROL_PACKAGE_SETUP_INPUTS(118),
+    RTDE_CONTROL_PACKAGE_START(83),
+    RTDE_CONTROL_PACKAGE_PAUSE(80)
+    ;
+
+    private final int val;
+    CommandType(int v) { val = v; }
+    public int getVal() { return val; }
+}

+ 21 - 0
src/main/java/at/acdp/urweb/rtde/InParams.java

@@ -0,0 +1,21 @@
+package at.acdp.urweb.rtde;
+
+public class InParams {
+    public static final String	speed_slider_mask	="speed_slider_mask"; //	UINT32	"0 = don't change speed slider with this input 1 = use speed_slider_fraction to set speed slider value"
+    public static final String	speed_slider_fraction	="speed_slider_fraction"; //	DOUBLE	new speed slider value
+    public static final String	standard_digital_output_mask	="standard_digital_output_mask"; //	UINT8	Standard digital output bit mask*
+    public static final String	standard_digital_output	="standard_digital_output"; //	UINT8	Standard digital outputs
+    public static final String	configurable_digital_output_mask	="configurable_digital_output_mask"; //	UINT8	Configurable digital output bit mask*
+    public static final String	configurable_digital_output	="configurable_digital_output"; //	UINT8	Configurable digital outputs
+    public static final String	tool_digital_output_mask	="tool_digital_output_mask"; //	UINT8	"Tool digital outputs mask  Bits 0-1: mask, remaining bits are reserved for future use"
+    public static final String	tool_digital_output	="tool_digital_output"; //	UINT8	"Tool digital outputs Bits 0-1: output state, remaining bits are reserved for future use"
+    public static final String	standard_analog_output_mask	="standard_analog_output_mask"; //	UINT8	"Standard analog output mask Bits 0-1: standard_analog_output_0 | standard_analog_output_1"
+    public static final String	standard_analog_output_type	="standard_analog_output_type"; //	UINT8	"Output domain {0=current[A], 1=voltage[V]} Bits 0-1: standard_analog_output_0 | standard_analog_output_1"
+    public static final String	standard_analog_output_0	="standard_analog_output_0"; //	DOUBLE	Standard analog output 0 (ratio) [0..1]
+    public static final String	standard_analog_output_1	="standard_analog_output_1"; //	DOUBLE	Standard analog output 1 (ratio) [0..1]
+    public static final String	input_bit_registers0_to_31	="input_bit_registers0_to_31"; //	UINT32	"General purpose bits This range of the boolean input registers is reserved for FieldBus/PLC interface usage."
+    public static final String	input_bit_registers32_to_63	="input_bit_registers32_to_63"; //	UINT32	"General purpose bits This range of the boolean input registers is reserved for FieldBus/PLC interface usage."
+    public static final String	input_bit_register_X	="input_bit_register_X"; //	BOOL	"64 general purpose bits X: [64..127] - The upper range of the boolean input registers can be used by external RTDE clients (i.e URCAPS)."
+    public static final String	input_int_register_X	="input_int_register_X"; //	INT32	"48 general purpose integer registers X: [0..23] - The lower range of the integer input registers is reserved for FieldBus/PLC interface usage. X: [24..47] - The upper range of the integer input registers can be used by external RTDE clients (i.e URCAPS)."
+    public static final String	input_double_register_X	="input_double_register_X"; //	DOUBLE	"48 general purpose double registers X: [0..23] - The lower range of the double input registers is reserved for FieldBus/PLC interface usage. X: [24..47] - The upper range of the double input registers can be used by external RTDE clients (i.e URCAPS)."
+}

+ 41 - 0
src/main/java/at/acdp/urweb/rtde/OutParams.java

@@ -0,0 +1,41 @@
+package at.acdp.urweb.rtde;
+
+public class OutParams {
+    public static final String	timestamp	="timestamp"; //	DOUBLE	Time elapsed since the controller was started [s]
+    public static final String	target_q	="target_q"; //	VECTOR6D	Target joint positions
+    public static final String	target_qd	="target_qd"; //	VECTOR6D	Target joint velocities
+    public static final String	target_qdd	="target_qdd"; //	VECTOR6D	Target joint accelerations
+    public static final String	target_current	="target_current"; //	VECTOR6D	Target joint currents
+    public static final String	target_moment	="target_moment"; //	VECTOR6D	Target joint moments (torques)
+    public static final String	actual_q	="actual_q"; //	VECTOR6D	Actual joint positions
+    public static final String	actual_qd	="actual_qd"; //	VECTOR6D	Actual joint velocities
+    public static final String	actual_current	="actual_current"; //	VECTOR6D	Actual joint currents
+    public static final String	joint_control_output	="joint_control_output"; //	VECTOR6D	Joint control currents
+    public static final String	actual_TCP_pose	="actual_TCP_pose"; //	VECTOR6D	Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
+    public static final String	actual_TCP_speed	="actual_TCP_speed"; //	VECTOR6D	Actual speed of the tool given in Cartesian coordinates
+    public static final String	actual_TCP_force	="actual_TCP_force"; //	VECTOR6D	Generalized forces in the TCP
+    public static final String	target_TCP_pose	="target_TCP_pose"; //	VECTOR6D	Target Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation
+    public static final String	target_TCP_speed	="target_TCP_speed"; //	VECTOR6D	Target speed of the tool given in Cartesian coordinates
+    public static final String	actual_digital_input_bits	="actual_digital_input_bits"; //	UINT64	Current state of the digital inputs. 0-7: Standard, 8-15: Configurable, 16-17: Tool
+    public static final String	joint_temperatures	="joint_temperatures"; //	VECTOR6D	Temperature of each joint in degrees Celsius
+    public static final String	actual_execution_time	="actual_execution_time"; //	DOUBLE	Controller real-time thread execution time
+    public static final String	robot_mode	="robot_mode"; //	INT32	Robot mode
+    public static final String	joint_mode	="joint_mode"; //	VECTOR6INT32	Joint control modes
+    public static final String	safety_mode	="safety_mode"; //	INT32	Safety mode
+    public static final String	safety_status	="safety_status"; //	INT32	Safety ststus
+    public static final String	actual_tool_accelerometer	="actual_tool_accelerometer"; //	VECTOR3D	Tool x, y and z accelerometer values
+    public static final String	speed_scaling	="speed_scaling"; //	DOUBLE	Speed scaling of the trajectory limiter
+    public static final String	target_speed_fraction	="target_speed_fraction"; //	DOUBLE	Target speed fraction
+    public static final String	actual_momentum	="actual_momentum"; //	DOUBLE	Norm of Cartesian linear momentum
+    public static final String	actual_main_voltage	="actual_main_voltage"; //	DOUBLE	Safety Control Board: Main voltage
+    public static final String	actual_robot_voltage	="actual_robot_voltage"; //	DOUBLE	Safety Control Board: Robot voltage (48V)
+    public static final String	actual_robot_current	="actual_robot_current"; //	DOUBLE	Safety Control Board: Robot current
+    public static final String	actual_joint_voltage	="actual_joint_voltage"; //	VECTOR6D	Actual joint voltages
+    public static final String	actual_digital_output_bits	="actual_digital_output_bits"; //	UINT64	Current state of the digital outputs. 0-7: Standard, 8-15: Configurable, 16-17: Tool
+    public static final String	runtime_state	="runtime_state"; //	UINT32	Program state
+    public static final String	elbow_position	="elbow_position"; //	VECTOR3D	Position of robot elbow in Cartesian Base Coordinates
+    public static final String	elbow_velocity	="elbow_velocity"; //	VECTOR3D	Velocity of robot elbow in Cartesian Base Coordinates
+    public static final String	robot_status_bits	="robot_status_bits"; //	UINT32	Bits 0-3: Is power on | Is program running | Is teach button pressed | Is power button pressed
+    public static final String	safety_status_bits	="safety_status_bits"; //	UINT32	Bits 0-10: Is normal mode | Is reduced mode | | Is protective stopped | Is recovery mode | Is safeguard stopped | Is system emergency stopped | Is robot emergency stopped | Is emergency stopped | Is violation | Is fault | Is stopped due to safety
+    public static final String	analog_io_types	="analog_io_types"; //	UINT32
+}

+ 100 - 0
src/main/java/at/acdp/urweb/rtde/RTDEClient.java

@@ -0,0 +1,100 @@
+package at.acdp.urweb.rtde;
+
+import net.openhft.chronicle.bytes.Bytes;
+
+import java.io.DataInputStream;
+import java.io.DataOutputStream;
+import java.io.IOException;
+import java.net.Socket;
+import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.List;
+
+public class RTDEClient implements Runnable {
+
+    private final String ip;
+    private final int port;
+    private volatile boolean _running=true;
+    private DataOutputStream dos;
+    private DataInputStream dis;
+
+
+    public RTDEClient(String ip, int port) {
+        this.ip=ip;
+        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);){
+            rt.setSoTimeout(0);
+            if (rt.isConnected()){
+                System.out.println("Connected to UR Realtime Client");
+            }
+            dis = new DataInputStream(rt.getInputStream());
+            dos = new DataOutputStream(rt.getOutputStream());
+
+
+            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
+    public void run() {
+        while(_running) {
+            try {
+                readSocket();
+            } catch (IOException e) {
+                e.printStackTrace();
+            }
+        }
+    }
+
+    public void send_input_setup(List<String> variables, List<String> types) {
+        var cmd = CommandType.RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
+        var payload = String.join(",", variables);
+        sendAndReceive(cmd, payload.getBytes());
+    }
+
+    private void sendAndReceive(CommandType cmd, byte[] payload) {
+        sendall(cmd, payload);
+    }
+
+
+    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 payload = String.join(",", variables);
+        sendAndReceive(cmd, payload.getBytes());
+    }
+
+    public void  sendall(CommandType cmd, byte[] payload) {
+        try {
+            Bytes<ByteBuffer> bytes = Bytes.elasticHeapByteBuffer(64);
+            int size = 3 + payload.length;
+            bytes.writeUnsignedShort(size);
+            bytes.writeUnsignedByte(cmd.getVal());
+            bytes.write(payload);
+            dos.write(bytes.toByteArray());
+            dos.flush();
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+    }
+}

+ 41 - 41
src/main/java/at/acdp/urweb/web/FestoXRServer.java

@@ -45,11 +45,7 @@ public class FestoXRServer {
                     logger.info("Reached {}", targetPos);
                     return "OK";
                 }
-                try {
-                    Thread.sleep(100);
-                } catch (InterruptedException e) {
-                    e.printStackTrace();
-                }
+                Thread.sleep(100);
             }
         } catch (ExecutionException|InterruptedException e) {
             logger.warn("abs failed", e);
@@ -62,27 +58,20 @@ public class FestoXRServer {
         try {
             logger.info("Absolute to {} speed {}", targetPos, speed);
             Main.fhm.bereitschaft();
-            if(!Main.fhm.readStatus().get().enabled) {
-                logger.info("Controller not enabled");
-                return "Controller not enabled";
-           }
-        Main.fhm.direktAuftrag(targetPos, speed, 0, false);
-        Thread.sleep(100);
-        Main.fhm.direktAuftrag(targetPos, speed, 1, false);
-
-        while(true) {
-            boolean reached=checkPos(targetPos);
-            if(reached) {
-                logger.info("Reached {}", targetPos);
-                return "OK";
-            }
-            try {
-                Thread.sleep(100);
-            } catch (InterruptedException e) {
-                e.printStackTrace();
+
+            Main.fhm.direktAuftrag(targetPos, speed, 0, false);
+            Thread.sleep(100);
+            Main.fhm.direktAuftrag(targetPos, speed, 1, false);
+
+            while(true) {
+                boolean reached=checkPos(targetPos);
+                if(reached) {
+                    logger.info("Reached {}", targetPos);
+                    return "OK";
+                }
+                    Thread.sleep(100);
             }
-        }
-        } catch (ExecutionException|InterruptedException e) {
+        } catch (InterruptedException e) {
             logger.warn("abs failed", e);
             return "abs failed: "+e.toString();
         }
@@ -102,10 +91,23 @@ public class FestoXRServer {
     }
 
     @XRMethod(value = "ready", help = "ready")
-    public static void ready() {
-        logger.info("Ready Begin");
-        Main.fhm.bereitschaft();
-        logger.info("Ready End");
+    public static String ready() {
+        try {
+            logger.info("Ready called");
+            Main.fhm.bereitschaft();
+            for(int i=0;i<5;i++) {
+                Status s=Main.fhm.readStatus().get();
+                if(s.enabled) {
+                    return "OK";
+                }
+                logger.info("Controller not enabled.." + i);
+                Thread.sleep(100);
+            }
+
+        } catch (InterruptedException|ExecutionException e) {
+            logger.warn("ready failed", e);
+        }
+        return "Controller not enabled";
     }
 
     private static boolean checkPos(int pos) {
@@ -123,10 +125,8 @@ public class FestoXRServer {
                     return true;
                 }
                 Thread.sleep(500);
-            } catch (ExecutionException e) {
-                e.printStackTrace();
-            } catch (InterruptedException e) {
-                e.printStackTrace();
+            } catch (ExecutionException|InterruptedException e) {
+                logger.warn("checkPos failed", e);
             }
         return false;
     }
@@ -139,14 +139,14 @@ public class FestoXRServer {
                 return null;
             var ret= x.get();
             Map<String, Object> m=new HashMap<>();
-
-            m.put("opm", ret.istMoment);
-            m.put("fct", ret.abs);
-            m.put("rdyen", true);
-            m.put("fault", ret.com);
-            m.put("warn", ret.dev);
-            m.put("open", ret.fault);
-            m.put("enabled", ret.fct);
+            m.put("opm1", ret.opm1);
+            m.put("opm2", ret.opm2);
+            m.put("fct", ret.fct);
+            m.put("rdyen", ret.rdyen);
+            m.put("fault", ret.fault);
+            m.put("warn", ret.warn);
+            m.put("open", ret.open);
+            m.put("enabled", ret.enabled);
 
             m.put("ref", ret.ref);
             m.put("still", ret.still);