Menu
▾
▴
Re: [Java-player-users] Actarray and Fiducial interface patch
|
From: Vitalijs K. <vit...@gm...> - 2010-09-27 13:48:37
|
Index: javaclient3/FiducialInterface.java
===================================================================
--- javaclient3/FiducialInterface.java (revision 99)
+++ javaclient3/FiducialInterface.java (working copy)
@@ -92,9 +92,13 @@
xdr.close ();
// Buffer for reading fiducials
- buffer = new byte[PLAYER_FIDUCIAL_MAX_SAMPLES * (4+24*2)];
+ // 4 bytes - ID
+ // 4*2 = 8 bytes - double field
+ // 6 fields in single PlayerPose3d object
+ // 2 PlayerPose3d objects
+ buffer = new byte[PLAYER_FIDUCIAL_MAX_SAMPLES * (4+4*2*6*2)];
// Read fiducials
- is.readFully (buffer, 0, fiducialsCount * (4+24*2));
+ is.readFully (buffer, 0, fiducialsCount * (4+4*2*6*2));
xdr = new XdrBufferDecodingStream (buffer);
xdr.beginDecoding ();
PlayerFiducialItem[] pfis = new PlayerFiducialItem[fiducialsCount];
@@ -105,20 +109,20 @@
PlayerPose3d pose = new PlayerPose3d ();
PlayerPose3d upose = new PlayerPose3d ();
- pose.setPx (xdr.xdrDecodeFloat ());
- pose.setPy (xdr.xdrDecodeFloat ());
- pose.setPz (xdr.xdrDecodeFloat ());
- pose.setProll (xdr.xdrDecodeFloat ());
- pose.setPpitch (xdr.xdrDecodeFloat ());
- pose.setPyaw (xdr.xdrDecodeFloat ());
+ pose.setPx (xdr.xdrDecodeDouble());
+ pose.setPy (xdr.xdrDecodeDouble());
+ pose.setPz (xdr.xdrDecodeDouble());
+ pose.setProll (xdr.xdrDecodeDouble());
+ pose.setPpitch (xdr.xdrDecodeDouble());
+ pose.setPyaw (xdr.xdrDecodeDouble());
- upose.setPx (xdr.xdrDecodeFloat ());
- upose.setPy (xdr.xdrDecodeFloat ());
- upose.setPz (xdr.xdrDecodeFloat ());
- upose.setProll (xdr.xdrDecodeFloat ());
- upose.setPpitch (xdr.xdrDecodeFloat ());
- upose.setPyaw (xdr.xdrDecodeFloat ());
-
+ upose.setPx (xdr.xdrDecodeDouble ());
+ upose.setPy (xdr.xdrDecodeDouble ());
+ upose.setPz (xdr.xdrDecodeDouble ());
+ upose.setProll (xdr.xdrDecodeDouble ());
+ upose.setPpitch (xdr.xdrDecodeDouble ());
+ upose.setPyaw (xdr.xdrDecodeDouble ());
+
pfi.setPose (pose);
pfi.setUpose (upose);
Index: javaclient3/structures/actarray/PlayerActarrayActuator.java
===================================================================
--- javaclient3/structures/actarray/PlayerActarrayActuator.java (revision 99)
+++ javaclient3/structures/actarray/PlayerActarrayActuator.java (working copy)
@@ -34,6 +34,10 @@
*/
public class PlayerActarrayActuator implements PlayerConstants {
+ // The acceleration of the actuator in m/s^2 or rad/s^2 depending on the type.
+ private float acceleration;
+ // The current of the actuator in A.
+ private float current;
// The position of the actuator in m or rad depending on the type.
private float position;
// The speed of the actuator in m/s or rad/s depending on the type.
@@ -85,4 +89,36 @@
this.state = newState;
}
-}
\ No newline at end of file
+ /**
+ * @return The acceleration of the actuator in m/s^2 or rad/s^2 depending on the type.
+ **/
+ public synchronized float getAcceleration() {
+ return acceleration;
+ }
+
+ /**
+ * @param newAcceleration The acceleration of the actuator in m/s^2 or rad/s^2 depending on the type.
+ *
+ */
+ public synchronized void setAcceleration(float newAcceleration) {
+ this.acceleration = newAcceleration;
+ }
+
+ /**
+ * @return The current of the actuator in A.
+ **/
+ public synchronized float getCurrent() {
+ return current;
+ }
+
+ /**
+ * @param newCurrent The current of the actuator in A.
+ *
+ */
+ public synchronized void setCurrent(float newCurrent) {
+ this.current = newCurrent;
+ }
+
+
+
+}
Index: javaclient3/structures/PlayerPose.java
===================================================================
--- javaclient3/structures/PlayerPose.java (revision 99)
+++ javaclient3/structures/PlayerPose.java (working copy)
@@ -95,4 +95,9 @@
public synchronized void setPa (double newPa) {
this.pa = newPa;
}
+
+ @Override
+ public String toString() {
+ return "PlayerPose{ [" + px + " " + py + " 0 " + Math.toDegrees(pa) + "]}";
+ }
}
\ No newline at end of file
Index: javaclient3/structures/PlayerConstants.java
===================================================================
--- javaclient3/structures/PlayerConstants.java (revision 99)
+++ javaclient3/structures/PlayerConstants.java (working copy)
@@ -247,9 +247,14 @@
public final int PLAYER_ACTARRAY_BRAKES_REQ = 2;
public final int PLAYER_ACTARRAY_GET_GEOM_REQ = 3;
public final int PLAYER_ACTARRAY_SPEED_REQ = 4;
+ public final int PLAYER_ACTARRAY_ACCEL_REQ = 5;
public final int PLAYER_ACTARRAY_POS_CMD = 1;
- public final int PLAYER_ACTARRAY_SPEED_CMD = 2;
- public final int PLAYER_ACTARRAY_HOME_CMD = 3;
+ public final int PLAYER_ACTARRAY_MULTI_POS_CMD = 2;
+ public final int PLAYER_ACTARRAY_SPEED_CMD = 3;
+ public final int PLAYER_ACTARRAY_MULTI_SPEED_CMD = 4;
+ public final int PLAYER_ACTARRAY_HOME_CMD = 5;
+ public final int PLAYER_ACTARRAY_CURRENT_CMD = 6;
+ public final int PLAYER_ACTARRAY_MULTI_CURRENT_CMD = 7;
public final int PLAYER_ACTARRAY_DATA_STATE = 1;
public final int PLAYER_AIO_MAX_INPUTS = 8;
Index: javaclient3/ActarrayInterface.java
===================================================================
--- javaclient3/ActarrayInterface.java (revision 99)
+++ javaclient3/ActarrayInterface.java (working copy)
@@ -82,19 +82,23 @@
xdr.close ();
// Buffer for reading actuators data
- buffer = new byte[PLAYER_ACTARRAY_NUM_ACTUATORS * 12];
+ buffer = new byte[PLAYER_ACTARRAY_NUM_ACTUATORS * 24];
// Read actuators data
- is.readFully (buffer, 0, actuatorsCount * 12);
+ is.readFully (buffer, 0, actuatorsCount * 24);
xdr = new XdrBufferDecodingStream (buffer);
xdr.beginDecoding ();
PlayerActarrayActuator[] paas = new PlayerActarrayActuator[actuatorsCount];
for (int i = 0; i < actuatorsCount; i++) {
PlayerActarrayActuator paa = new PlayerActarrayActuator ();
+ //unknown int field, always = 1, probably PLAYER_ACTARRAY_DATA_STATE
+ xdr.xdrDecodeInt();
paa.setPosition (xdr.xdrDecodeFloat ());
paa.setSpeed (xdr.xdrDecodeFloat ());
+ paa.setAcceleration(xdr.xdrDecodeFloat ());
+ paa.setCurrent (xdr.xdrDecodeFloat ());
paa.setState (xdr.xdrDecodeByte ());
-
+
paas[i] = paa;
}
xdr.endDecoding ();
@@ -157,7 +161,7 @@
*/
public void setSpeed (int joint, float speed) {
try {
- sendHeader (PLAYER_MSGTYPE_CMD, PLAYER_ACTARRAY_POS_CMD, 8);
+ sendHeader (PLAYER_MSGTYPE_CMD, PLAYER_ACTARRAY_SPEED_CMD, 8);
XdrBufferEncodingStream xdr = new XdrBufferEncodingStream (8);
xdr.beginEncoding (null, 0);
xdr.xdrEncodeByte ((byte)joint); |
