|
@@ -38,7 +38,6 @@ public class SecondaryClient {
|
|
int beforeCount=di.getCount();
|
|
int beforeCount=di.getCount();
|
|
int size = di.readInt(); //4
|
|
int size = di.readInt(); //4
|
|
int pType = di.readByte() & 0xff; //+1=5
|
|
int pType = di.readByte() & 0xff; //+1=5
|
|
- log.info("ptype: "+pType);
|
|
|
|
switch (pType) {
|
|
switch (pType) {
|
|
case MessageType.ROBOT_MESSAGE:
|
|
case MessageType.ROBOT_MESSAGE:
|
|
readRobotMessage(di, size);
|
|
readRobotMessage(di, size);
|
|
@@ -47,31 +46,40 @@ public class SecondaryClient {
|
|
readRobotState(di, size);
|
|
readRobotState(di, size);
|
|
break;
|
|
break;
|
|
case PackageType.ROBOT_MODE_DATA:
|
|
case PackageType.ROBOT_MODE_DATA:
|
|
- readRobotModeData(di, size);
|
|
|
|
|
|
+ var md=readRobotModeData(di, size);
|
|
|
|
+ log.info(md.toString());
|
|
break;
|
|
break;
|
|
case PackageType.JOINT_DATA:
|
|
case PackageType.JOINT_DATA:
|
|
- readJointData(di, size);
|
|
|
|
|
|
+ var jd=readJointData(di, size);
|
|
|
|
+ log.info(jd.toString());
|
|
break;
|
|
break;
|
|
case PackageType.TOOL_DATA:
|
|
case PackageType.TOOL_DATA:
|
|
- readToolData(di, size);
|
|
|
|
|
|
+ var td=readToolData(di, size);
|
|
|
|
+ log.info(td.toString());
|
|
break;
|
|
break;
|
|
case PackageType.MASTERBOARD_DATA:
|
|
case PackageType.MASTERBOARD_DATA:
|
|
- readMasterBoardData(di, size);
|
|
|
|
|
|
+ var mb=readMasterBoardData(di, size);
|
|
|
|
+ log.info(mb.toString());
|
|
break;
|
|
break;
|
|
case PackageType.CARTESIAN_INFO:
|
|
case PackageType.CARTESIAN_INFO:
|
|
- readCartesianInfo(di, size);
|
|
|
|
|
|
+ var c=readCartesianInfo(di, size);
|
|
|
|
+ log.info(c.toString());
|
|
break;
|
|
break;
|
|
case PackageType.KINEMATICS_INFO:
|
|
case PackageType.KINEMATICS_INFO:
|
|
- readKinemsticsInfo(di, size);
|
|
|
|
|
|
+ var ki=readKinemsticsInfo(di, size);
|
|
|
|
+ log.info(ki.toString());
|
|
break;
|
|
break;
|
|
case PackageType.CONFIGURATION_DATA:
|
|
case PackageType.CONFIGURATION_DATA:
|
|
- readConfigurationData(di, size);
|
|
|
|
|
|
+ var cd=readConfigurationData(di, size);
|
|
|
|
+ log.info(cd.toString());
|
|
break;
|
|
break;
|
|
case PackageType.FORCE_MODE_DATA:
|
|
case PackageType.FORCE_MODE_DATA:
|
|
- readForceModeData(di, size);
|
|
|
|
|
|
+ var fmd=readForceModeData(di, size);
|
|
|
|
+ log.info(fmd.toString());
|
|
break;
|
|
break;
|
|
case PackageType.ADDITIONAL_INFO:
|
|
case PackageType.ADDITIONAL_INFO:
|
|
- readAdditionalInfo(di, size);
|
|
|
|
|
|
+ var ai=readAdditionalInfo(di, size);
|
|
|
|
+ log.info(ai.toString());
|
|
break;
|
|
break;
|
|
case PackageType.NEEDED_FOR_CALIB_DATA:
|
|
case PackageType.NEEDED_FOR_CALIB_DATA:
|
|
skip(PackageType.NEEDED_FOR_CALIB_DATA,di, size);
|
|
skip(PackageType.NEEDED_FOR_CALIB_DATA,di, size);
|
|
@@ -80,7 +88,8 @@ public class SecondaryClient {
|
|
skip(PackageType.SAFETY_DATA,di, size);
|
|
skip(PackageType.SAFETY_DATA,di, size);
|
|
break;
|
|
break;
|
|
case PackageType.TOOL_COMM_INFO:
|
|
case PackageType.TOOL_COMM_INFO:
|
|
- readToolCommInfo(di, size);
|
|
|
|
|
|
+ var tci=readToolCommInfo(di, size);
|
|
|
|
+ log.info(tci.toString());
|
|
break;
|
|
break;
|
|
|
|
|
|
|
|
|
|
@@ -146,7 +155,7 @@ public class SecondaryClient {
|
|
char robotMessageType = (char) (di.readByte() & 0xFF);
|
|
char robotMessageType = (char) (di.readByte() & 0xFF);
|
|
switch (robotMessageType) {
|
|
switch (robotMessageType) {
|
|
default:
|
|
default:
|
|
- System.out.println("unknown msg" + (int) robotMessageType);
|
|
|
|
|
|
+ log.warn("unknown msg" + (int) robotMessageType);
|
|
//byte[] buf=new byte[size-11];
|
|
//byte[] buf=new byte[size-11];
|
|
byte[] buf = new byte[size - 10];
|
|
byte[] buf = new byte[size - 10];
|
|
di.readFully(buf);
|
|
di.readFully(buf);
|
|
@@ -160,7 +169,7 @@ public class SecondaryClient {
|
|
char robotMessageType = (char) (di.readByte() & 0xFF);
|
|
char robotMessageType = (char) (di.readByte() & 0xFF);
|
|
switch (robotMessageType) {
|
|
switch (robotMessageType) {
|
|
default:
|
|
default:
|
|
- System.out.println("unknown msg" + robotMessageType);
|
|
|
|
|
|
+ log.info("unknown msg: " + (int)robotMessageType);
|
|
byte[] buf = new byte[size - 10];
|
|
byte[] buf = new byte[size - 10];
|
|
di.readFully(buf);
|
|
di.readFully(buf);
|
|
}
|
|
}
|
|
@@ -194,15 +203,11 @@ public class SecondaryClient {
|
|
md.speedScaling = di.readDouble();
|
|
md.speedScaling = di.readDouble();
|
|
md.targetSpeedFractionLimit = di.readDouble();
|
|
md.targetSpeedFractionLimit = di.readDouble();
|
|
md.internal = di.readByte();
|
|
md.internal = di.readByte();
|
|
- //byte[] buf=new byte[50];
|
|
|
|
- //di.readFully(buf);
|
|
|
|
return md;
|
|
return md;
|
|
}
|
|
}
|
|
|
|
|
|
JointData[] readJointData(CountDataInputStream di, int size) throws IOException {
|
|
JointData[] readJointData(CountDataInputStream di, int size) throws IOException {
|
|
int joints = size / 41;
|
|
int joints = size / 41;
|
|
- // byte[] buf=new byte[300];
|
|
|
|
- // di.readFully(buf);
|
|
|
|
JointData[] jds = new JointData[size];
|
|
JointData[] jds = new JointData[size];
|
|
for (int i = 0; i < joints; i++) {
|
|
for (int i = 0; i < joints; i++) {
|
|
JointData jd = new JointData();
|
|
JointData jd = new JointData();
|