parse modedata and jointdata (wip)

src/main/java/at/acdp/urweb/Main.java

@@ -12,11 +12,12 @@ public class Main {
         try {
             new WebServer(8080,true).start();
             //GetRobotRealtimeData rd=new GetRobotRealtimeData("192.168.20.107", 30003);
-            SecondaryClient rde=new SecondaryClient("192.168.20.144", 30002);
+            SecondaryClient rde=new SecondaryClient("192.168.20.156", 30002);
             rde.connect();
             rde.writeCmd("set_digital_out(1,True)\n");
 
 
+
             // rde.writeCmd("set_digital_out(2,True)\n");
             // rde.writeCmd("movej([-1.95,-1.58,-1.16,-1.15,-1.55,1.25], a=1.0, v=0.1)\n");
             //rde.writeCmd("freedrive_mode()\n");

src/main/java/at/acdp/urweb/sclient/JointData.java

@@ -0,0 +1,27 @@
+package at.acdp.urweb.sclient;
+
+import java.io.DataInputStream;
+import java.io.IOException;
+
+//49 byte
+public class JointData {
+    double qActual;
+    double qTarget;
+    double qdActual;
+    float lActual;
+    double vActual;
+    float tMotor;
+    double tMicro;
+    byte jointMode;
+
+    public void read(DataInputStream di) throws IOException {
+        qActual=di.readDouble();
+        qTarget=di.readDouble();
+        qdActual=di.readDouble();
+        lActual=di.readFloat();
+        vActual=di.readDouble();
+        tMotor=di.readFloat();
+        tMicro=di.readDouble();
+        jointMode=di.readByte();
+    }
+}

src/main/java/at/acdp/urweb/sclient/ModeData.java

@@ -0,0 +1,17 @@
+package at.acdp.urweb.sclient;
+
+public class ModeData {
+    public boolean realRobotConnected;
+    public boolean realRobotEnabled;
+    public boolean robotPoweredOn;
+    public boolean emergencyStopped;
+    public boolean protectiveStopped;
+    public boolean isProgramRunning;
+    public boolean isProgramPaused;
+    public byte robotMode;
+    public byte controlMode;
+    public double targetSpeedFraction;
+    public double speedScaling;
+    public double targetSpeedFractionLimit;
+    public int internal;
+}

src/main/java/at/acdp/urweb/sclient/SecondaryClient.java

@@ -1,6 +1,8 @@
 package at.acdp.urweb.sclient;
 
 import at.acdp.urweb.sclient.data.MessageType;
+import at.acdp.urweb.sclient.data.PackageType;
+import at.acdp.urweb.sclient.data.RobotMessageType;
 import at.acdp.urweb.sclient.data.VersionMessage;
 
 import java.io.*;
@@ -28,59 +30,96 @@ public class SecondaryClient {
         VersionMessage vm = new VersionMessage();
         vm.readVersionMessage(in);
         System.out.println(vm);
-        while(in.available()>0) {
-            readReply(in);
-
-        }
+        readReply(in);
     }
 
     private void readReply(DataInputStream di) throws IOException {
-        byte[] asdf=new byte[150];
-        //di.readFully(asdf);
-        System.out.println(asdf);
 
         int size=di.readInt(); //4
-        int mType= di.readByte() &0xff; //+1=5
-        switch (mType) {
+        System.out.println("size: " +size);
+        int pType= di.readByte() &0xff; //+1=5
+        switch (pType) {
             case MessageType.ROBOT_MESSAGE:
-                long ts=di.readLong();//+8=11
-                byte source=di.readByte(); //+1=12
-                int robotMessageType=di.readByte() & 0xff; //+1=13
-                switch(robotMessageType) {
-
-                    default:
-                        System.out.println("rtype: " +robotMessageType);
-                        int x=di.available();
-                        byte[] buf=new byte[size-10];
-                        di.readFully(buf);
-                        System.out.println(buf);
-                }
+                readRobotMessage(di, size);
+                break;
+            case MessageType.ROBOT_STATE:
+                readRobotState(di, size);
+                break;
+            case PackageType.ROBOT_MODE_DATA:
+                readRobotModeData(di, size);
                 break;
-            case 16:
-                System.out.println("16 msg" + mType);
-                int remaining=size-5;
-                while (remaining >0) {
-                    int sublength = in.readInt();
-                    remaining-=sublength;
-                    System.out.println("sublength: " + sublength);
-                    int subType = in.readByte();
-                    System.out.println("subtype: " + subType);
-                    byte[] res = new byte[sublength - 5];
-                    in.read(res);
-                }
+            case PackageType.JOINT_DATA:
+                readJointData(di, size);
                 break;
+        }
+    }
+    void readRobotMessage(DataInputStream di, int size) throws IOException {
+        long ts=di.readLong();
+        char source= (char) (di.readByte() & 0xFF);
+        char robotMessageType= (char) (di.readByte() & 0xFF);
+        switch (robotMessageType) {
+            default:
+                System.out.println("unknown msg" + (int)robotMessageType);
+                //byte[] buf=new byte[size-11];
+                byte[] buf=new byte[size-10];
+                di.readFully(buf);
+                System.out.println(buf);
+        }
+    }
+
+    void readRobotState(DataInputStream di, int size) throws IOException {
+        long ts=di.readLong();
+        char source= (char) (di.readByte() & 0xFF);
+        char robotMessageType= (char) (di.readByte() & 0xFF);
+        switch (robotMessageType) {
             default:
-                System.out.println("unknown msg" + mType);
-                byte[] buf=new byte[size-5];
+                System.out.println("unknown msg" + robotMessageType);
+                byte[] buf=new byte[size-10];
                 di.readFully(buf);
         }
     }
 
-    public void writeCmd(String cmd) {
+    ModeData readRobotModeData(DataInputStream di, int size) throws IOException {
+        long ts=di.readLong();
+        ModeData md=new ModeData();
+        md.realRobotConnected=di.readBoolean();
+        md.realRobotEnabled=di.readBoolean();
+        md.robotPoweredOn=di.readBoolean();
+        md.emergencyStopped=di.readBoolean();
+        md.protectiveStopped=di.readBoolean();
+        md.isProgramRunning=di.readBoolean();
+        md.isProgramPaused=di.readBoolean();
+        md.robotMode=di.readByte();
+        md.controlMode=di.readByte();
+        md.targetSpeedFraction=di.readDouble();
+        md.speedScaling=di.readDouble();
+        md.targetSpeedFractionLimit=di.readDouble();
+        md.internal=di.readByte();
+        //byte[] buf=new byte[50];
+        //di.readFully(buf);
+        return md;
+    }
+
+    JointData[] readJointData(DataInputStream di, int size) throws IOException {
+        int joints=size/49;
+        JointData[] jds=new JointData[5];
+        for(int i=0;i<joints;i++) {
+            JointData jd=new JointData();
+            jd.read(di);
+            jds[i]=jd;
+        }
+        return jds;
+    }
+
+
+
+        public void writeCmd(String cmd) {
         try {
             System.out.println("send cmd:" + cmd);
             os.write(cmd.getBytes(StandardCharsets.UTF_8));
             readReply(in);
+            readReply(in);
+            readReply(in);
         } catch (IOException e) {
             e.printStackTrace();
         }

src/main/java/at/acdp/urweb/sclient/data/MessageType.java

@@ -2,5 +2,12 @@ package at.acdp.urweb.sclient.data;
 
 public class MessageType {
     public static final int ROBOT_MESSAGE=20;
+    public static final int ROBOT_STATE=16;
+    public static final int HMC_MESSAGE=22;
+    public static final int MODBUS_INFO_MESSAGE=5;
+    public static final int SAFETY_SETUP_BROADCAST_MESSAGE=23;
+    public static final int SAFETY_COMPLIANCE_TOLERANCES_MESSAGE=24;
     public static final int PROGRAM_STATE_MESSAGE=25;
+
+
 }

src/main/java/at/acdp/urweb/sclient/data/PackageType.java

@@ -1,13 +1,16 @@
 package at.acdp.urweb.sclient.data;
 
 public class PackageType {
-    public static final int CARTESIAN_INFO=4;
-    public static final int MASTERBOARD_DATA=3;
+    public static final int ROBOT_MODE_DATA=0;
+    public static final int JOINT_DATA=1;
     public static final int TOOL_DATA=2;
+    public static final int MASTERBOARD_DATA=3;
+    public static final int CARTESIAN_INFO=4;
     public static final int KINEMATICS_INFO=5;
     public static final int CONFIGURATION_DATA=6;
     public static final int FORCE_MODE_DATA=7;
     public static final int ADDITIONAL_INFO=8;
     public static final int NEEDED_FOR_CALIB_DATA=9;
+    public static final int SAFETY_DATA=10;
     public static final int TOOL_COMM_INFO=11;
 }