parse jointdata

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

@@ -1,9 +1,6 @@
 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 at.acdp.urweb.sclient.data.*;
 
 import java.io.*;
 import java.net.Socket;
@@ -35,9 +32,9 @@ public class SecondaryClient {
 
     private void readReply(DataInputStream di) throws IOException {
 
-        int size=di.readInt(); //4
-        System.out.println("size: " +size);
-        int pType= di.readByte() &0xff; //+1=5
+        int size = di.readInt(); //4
+        System.out.println("size: " + size);
+        int pType = di.readByte() & 0xff; //+1=5
         switch (pType) {
             case MessageType.ROBOT_MESSAGE:
                 readRobotMessage(di, size);
@@ -51,69 +48,80 @@ public class SecondaryClient {
             case PackageType.JOINT_DATA:
                 readJointData(di, size);
                 break;
+            case PackageType.CARTESIAN_INFO:
+                readCartesianInfo(di, size);
         }
     }
+
     void readRobotMessage(DataInputStream di, int size) throws IOException {
-        long ts=di.readLong();
-        char source= (char) (di.readByte() & 0xFF);
-        char robotMessageType= (char) (di.readByte() & 0xFF);
+        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);
+                System.out.println("unknown msg" + (int) robotMessageType);
                 //byte[] buf=new byte[size-11];
-                byte[] buf=new byte[size-10];
+                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);
+        long ts = di.readLong();
+        char source = (char) (di.readByte() & 0xFF);
+        char robotMessageType = (char) (di.readByte() & 0xFF);
         switch (robotMessageType) {
             default:
                 System.out.println("unknown msg" + robotMessageType);
-                byte[] buf=new byte[size-10];
+                byte[] buf = new byte[size - 10];
                 di.readFully(buf);
         }
     }
 
+    CartesianInfo readCartesianInfo(DataInputStream di, int size) throws IOException {
+        CartesianInfo ci = new CartesianInfo();
+        //TODO: parse
+        return ci;
+
+    }
+
     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();
+        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();
+        int joints = size / 41;
+        // byte[] buf=new byte[300];
+        // di.readFully(buf);
+        JointData[] jds = new JointData[size];
+        for (int i = 0; i < joints; i++) {
+            JointData jd = new JointData();
             jd.read(di);
-            jds[i]=jd;
+            jds[i] = jd;
         }
         return jds;
     }
 
 
-
-        public void writeCmd(String cmd) {
+    public void writeCmd(String cmd) {
         try {
             System.out.println("send cmd:" + cmd);
             os.write(cmd.getBytes(StandardCharsets.UTF_8));

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

@@ -0,0 +1,4 @@
+package at.acdp.urweb.sclient.data;
+
+public class CartesianInfo {
+}

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

@@ -1,4 +1,4 @@
-package at.acdp.urweb.sclient;
+package at.acdp.urweb.sclient.data;
 
 import java.io.DataInputStream;
 import java.io.IOException;
@@ -14,14 +14,16 @@ public class JointData {
     double tMicro;
     byte jointMode;
 
+    //TODO: float/double propably wrong
+    //total length 41 bytes
     public void read(DataInputStream di) throws IOException {
         qActual=di.readDouble();
         qTarget=di.readDouble();
         qdActual=di.readDouble();
         lActual=di.readFloat();
-        vActual=di.readDouble();
+        vActual=di.readFloat();
         tMotor=di.readFloat();
-        tMicro=di.readDouble();
+        tMicro=di.readFloat();
         jointMode=di.readByte();
     }
 }

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

@@ -1,4 +1,4 @@
-package at.acdp.urweb.sclient;
+package at.acdp.urweb.sclient.data;
 
 public class ModeData {
     public boolean realRobotConnected;