From 84342385d19d9c6f62d0cc9f7d7929fa5dfa8ca4 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Thu, 8 Sep 2016 17:49:45 -0400 Subject: [PATCH 01/25] Added new Hillcrest IMU node Essentially this is replacing the old IMU nodes by taking what we learned about the Hillcrest IMU and cleaning it up, complete with unit tests --- .../nodes/sensors/HillcrestNode.java | 30 +++++++++++++++++++ .../nodes/sensors/HillcrestNodeTest.java | 18 +++++++++++ 2 files changed, 48 insertions(+) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java create mode 100644 real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java new file mode 100644 index 00000000..94bd9b87 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java @@ -0,0 +1,30 @@ +package com.roboclub.robobuggy.nodes.sensors; + +import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; + +/** + * A node to communicate with the Hillcrest Freespace 9DOF IMU + */ +public class HillcrestNode extends BuggyDecoratorNode { + + /** + * Creates a new decorator for the given {@link Node} + * + * @param node {@link Node} to decorate + * @param name the name we want for this node to store so that it can be referenced later + */ + public HillcrestNode(BuggyNode node, String name) { + super(node, name); + } + + @Override + protected boolean startDecoratorNode() { + return false; + } + + @Override + protected boolean shutdownDecoratorNode() { + return false; + } +} diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java new file mode 100644 index 00000000..75e20c02 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java @@ -0,0 +1,18 @@ +package com.roboclub.robobuggy.nodes.sensors; + +import junit.framework.TestCase; + +/** + * Created by vivaanbahl on 9/8/16. + */ +public class HillcrestNodeTest extends TestCase { + public void setUp() throws Exception { + super.setUp(); + + } + + public void tearDown() throws Exception { + + } + +} \ No newline at end of file From beb152cebf1b6d028a781ec2f4c1805e0f5b29c6 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 16 Sep 2016 14:21:39 -0400 Subject: [PATCH 02/25] Deprecated old IMU classes --- .../com/roboclub/robobuggy/messages/HillCrestIMUMessage.java | 1 + .../roboclub/robobuggy/messages/IMUAngularPositionMessage.java | 1 + .../roboclub/robobuggy/messages/IMUAngularVelocityMessage.java | 1 + .../robobuggy/messages/IMULinearAccelerationMessage.java | 1 + .../com/roboclub/robobuggy/messages/IMUTemperatureMessage.java | 1 + .../java/com/roboclub/robobuggy/messages/ImuMeasurement.java | 1 + .../com/roboclub/robobuggy/messages/MagneticMeasurement.java | 1 + .../com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java | 1 + .../main/java/com/roboclub/robobuggy/nodes/sensors/ImuNode.java | 1 + 9 files changed, 9 insertions(+) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java index 0aa9fe22..56506515 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java @@ -5,6 +5,7 @@ * * @author Trevor Decker */ +@Deprecated public class HillCrestIMUMessage extends BaseMessage { private String message = ""; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularPositionMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularPositionMessage.java index ab01e0e0..66e5538c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularPositionMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularPositionMessage.java @@ -12,6 +12,7 @@ *

* DESCRIPTION: TODO */ +@Deprecated public class IMUAngularPositionMessage extends BaseMessage { public static final String VERSION_ID = "imuAngularPositionV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularVelocityMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularVelocityMessage.java index 87927c22..a4cd6c57 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularVelocityMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularVelocityMessage.java @@ -12,6 +12,7 @@ *

* DESCRIPTION: TODO */ +@Deprecated public class IMUAngularVelocityMessage extends BaseMessage { public static final String VERSION_ID = "imuAngularVelocityV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMULinearAccelerationMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMULinearAccelerationMessage.java index eeea6133..cf28643c 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMULinearAccelerationMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMULinearAccelerationMessage.java @@ -12,6 +12,7 @@ *

* DESCRIPTION: TODO */ +@Deprecated public class IMULinearAccelerationMessage extends BaseMessage { public static final String VERSION_ID = "LinearAccelerationV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUTemperatureMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUTemperatureMessage.java index 7f44fbdb..68d02dbe 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUTemperatureMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUTemperatureMessage.java @@ -12,6 +12,7 @@ *

* DESCRIPTION: TODO */ +@Deprecated public class IMUTemperatureMessage extends BaseMessage { public static final String VERSION_ID = "temperatureV0.1"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/ImuMeasurement.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/ImuMeasurement.java index 74553781..5c86ef4d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/ImuMeasurement.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/ImuMeasurement.java @@ -12,6 +12,7 @@ *

* DESCRIPTION: TODO */ +@Deprecated public class ImuMeasurement extends BaseMessage { public static final String VERSION_ID = "imuV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/MagneticMeasurement.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/MagneticMeasurement.java index c316f92a..d978a439 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/MagneticMeasurement.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/MagneticMeasurement.java @@ -12,6 +12,7 @@ *

* DESCRIPTION: TODO */ +@Deprecated public class MagneticMeasurement extends BaseMessage { public static final String VERSION_ID = "magV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java index c3b6e557..ccad81c3 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java @@ -22,6 +22,7 @@ * @author Trevor Decker * @author Sean Buckley */ +@Deprecated public class HillCrestImuNode implements DiscoveryListenerInterface, DeviceListenerInterface, com.roboclub.robobuggy.ros.Node { private Device thisDevice; private Publisher linearAccPub = new Publisher(NodeChannel.IMU_LINEAR_ACC.getMsgPath()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/ImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/ImuNode.java index ad2344ba..4c48f815 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/ImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/ImuNode.java @@ -18,6 +18,7 @@ * @author Kevin Brennan * @author Trevor Decker */ +@Deprecated public final class ImuNode extends SerialNode { /** * Baud rate for serial port From 466938aaa8c1c26ef2440b5ebe686ef83d3aa953 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 4 Oct 2016 00:11:01 -0400 Subject: [PATCH 03/25] Added configuration for Hillcrest node Need to modify it to support multiple IMUs --- .../nodes/sensors/HillcrestNode.java | 77 ++++++++++++++++++- 1 file changed, 74 insertions(+), 3 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java index 94bd9b87..259d244d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java @@ -1,12 +1,27 @@ package com.roboclub.robobuggy.nodes.sensors; +import com.hcrest.jfreespace.Device; +import com.hcrest.jfreespace.DeviceListenerInterface; +import com.hcrest.jfreespace.Discovery; +import com.hcrest.jfreespace.DiscoveryListenerInterface; +import com.hcrest.jfreespace.FreespaceErrorCodes; +import com.hcrest.jfreespace.inreport.HidInMsg; +import com.hcrest.jfreespace.outreport.FreespaceMsgOutDataModeControlV2Request; +import com.hcrest.jfreespace.outreport.HidOutMsg; +import com.roboclub.robobuggy.main.RobobuggyLogicNotification; +import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.messages.ImuMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; /** * A node to communicate with the Hillcrest Freespace 9DOF IMU */ -public class HillcrestNode extends BuggyDecoratorNode { +public class HillcrestNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { + + private Device hillcrestImu; /** * Creates a new decorator for the given {@link Node} @@ -16,15 +31,71 @@ public class HillcrestNode extends BuggyDecoratorNode { */ public HillcrestNode(BuggyNode node, String name) { super(node, name); + } @Override protected boolean startDecoratorNode() { - return false; + // look for an IMU, and add this as a listener + Discovery.getInstance().addListener(this); + + return true; } @Override protected boolean shutdownDecoratorNode() { - return false; + Discovery.getInstance().removeListener(this); + return true; + } + + @Override + public void handleRead(Device device, HidInMsg hidInMsg, int i, long l) { + + } + + @Override + public void handleSend(Device device, HidOutMsg hidOutMsg, int i) { + + } + + @Override + public void notifyRemoved(Device device) { + + } + + @Override + public void freespaceDeviceInserted(Device device) { + hillcrestImu = device; + hillcrestImu.open(this); + + int modeAndStatus = 8; // bit 3 set gives Full Motion On mode - page 41 + int packetSelect = 8; // 8 = MotionEngineOutput - page 41 + int formatSelect = 1; // Mode 1 - page 18 + FreespaceMsgOutDataModeControlV2Request configMsg = new FreespaceMsgOutDataModeControlV2Request(); + + configMsg.setModeAndStatus(modeAndStatus); + configMsg.setPacketSelect(packetSelect); + configMsg.setFormatSelect(formatSelect); + + // get all the data + configMsg.setFf0(true); + configMsg.setFf1(true); + configMsg.setFf2(true); + configMsg.setFf3(true); + configMsg.setFf4(true); + configMsg.setFf5(true); + configMsg.setFf6(true); + configMsg.setFf7(true); + + FreespaceErrorCodes response = hillcrestImu.sendMessage(configMsg); + if (!response.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { + new RobobuggyLogicNotification("Error configuring IMU!", RobobuggyMessageLevel.EXCEPTION); + } + } + + @Override + public void freespaceDeviceRemoved(Device device) { + hillcrestImu.close(); + hillcrestImu = null; } } From 3ecb97d0bed55eff4d6045cfa21e70b6e7a492c0 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 4 Nov 2016 19:15:35 -0400 Subject: [PATCH 04/25] fixed testing issues --- .../nodes/sensors/HillcrestNode.java | 14 +++-------- .../nodes/sensors/HillcrestNodeTest.java | 25 ++++++++++++++++--- 2 files changed, 25 insertions(+), 14 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java index 259d244d..132b2980 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java @@ -10,11 +10,9 @@ import com.hcrest.jfreespace.outreport.HidOutMsg; import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; -import com.roboclub.robobuggy.messages.ImuMeasurement; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; -import com.roboclub.robobuggy.nodes.baseNodes.BuggyNode; import com.roboclub.robobuggy.ros.NodeChannel; -import com.roboclub.robobuggy.ros.Publisher; /** * A node to communicate with the Hillcrest Freespace 9DOF IMU @@ -24,14 +22,10 @@ public class HillcrestNode extends BuggyDecoratorNode implements DeviceListenerI private Device hillcrestImu; /** - * Creates a new decorator for the given {@link Node} - * - * @param node {@link Node} to decorate - * @param name the name we want for this node to store so that it can be referenced later + * Creates a new Hillcrest IMU node */ - public HillcrestNode(BuggyNode node, String name) { - super(node, name); - + public HillcrestNode() { + super(new BuggyBaseNode(NodeChannel.IMU), "Hillcrest IMU"); } @Override diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java index 75e20c02..4671e736 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java @@ -1,18 +1,35 @@ package com.roboclub.robobuggy.nodes.sensors; -import junit.framework.TestCase; +import org.junit.After; +import org.junit.Before; +import org.junit.Test; + +import static org.junit.Assert.assertEquals; /** * Created by vivaanbahl on 9/8/16. */ -public class HillcrestNodeTest extends TestCase { - public void setUp() throws Exception { - super.setUp(); +public class HillcrestNodeTest { + @Before + public void setUp() throws Exception { } + @After public void tearDown() throws Exception { } + /** + * Tests that we can safely create an instance of the node + * + * No input arguments + * Expects no output or errors + */ + @Test + public void testCreation() { + HillcrestNode imu = new HillcrestNode(); + assertEquals(imu.getName(), "Hillcrest IMU"); + } + } \ No newline at end of file From f4c66f94dfb695dfc32f691fb615d00816b5767b Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 18:26:14 -0500 Subject: [PATCH 05/25] fixed checkstyle issues --- .../robobuggy/nodes/sensors/HillcrestNodeTest.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java index 4671e736..76fff61a 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java @@ -11,12 +11,18 @@ */ public class HillcrestNodeTest { + /** + * setup done before each test + */ @Before - public void setUp() throws Exception { + public void setUp() { } + /** + * work that's done after each test + */ @After - public void tearDown() throws Exception { + public void tearDown() { } From 683d520e02fff7cc50eac103ed6e7de9a26beb68 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 22:42:54 -0500 Subject: [PATCH 06/25] finished up cleanup and added qn test cases --- .../messages/IMUAngularPositionMessage.java | 1 - .../messages/IMUAngularVelocityMessage.java | 1 - .../IMULinearAccelerationMessage.java | 1 - .../messages/MagneticMeasurement.java | 1 - .../nodes/sensors/HillCrestImuNode.java | 1 + .../nodes/sensors/HillcrestNode.java | 280 +++++++++++++++++- .../nodes/sensors/HillcrestNodeTest.java | 61 +++- 7 files changed, 336 insertions(+), 10 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularPositionMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularPositionMessage.java index b6b49ebc..3d5c938f 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularPositionMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularPositionMessage.java @@ -5,7 +5,6 @@ /** * Message for passing IMU angular positions measurements within BuggyROS */ -@Deprecated public class IMUAngularPositionMessage extends BaseMessage { public static final String VERSION_ID = "imuAngularPositionV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularVelocityMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularVelocityMessage.java index 38a84ccc..fdf65b3d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularVelocityMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMUAngularVelocityMessage.java @@ -5,7 +5,6 @@ /** * Message for passing IMU angular velocity measurements within BuggyROS */ -@Deprecated public class IMUAngularVelocityMessage extends BaseMessage { public static final String VERSION_ID = "imuAngularVelocityV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMULinearAccelerationMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMULinearAccelerationMessage.java index c4a367a1..ea00d4f2 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMULinearAccelerationMessage.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/IMULinearAccelerationMessage.java @@ -6,7 +6,6 @@ * Message for passing IMU linear acceleration measurements within BuggyROS * */ -@Deprecated public class IMULinearAccelerationMessage extends BaseMessage { public static final String VERSION_ID = "LinearAccelerationV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/MagneticMeasurement.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/MagneticMeasurement.java index d978a439..c316f92a 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/MagneticMeasurement.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/MagneticMeasurement.java @@ -12,7 +12,6 @@ *

* DESCRIPTION: TODO */ -@Deprecated public class MagneticMeasurement extends BaseMessage { public static final String VERSION_ID = "magV0.0"; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java index 0115fdd8..425493ea 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java @@ -25,6 +25,7 @@ * @author Sean Buckley */ @Deprecated +@SuppressWarnings("Duplicates") public class HillCrestImuNode implements DiscoveryListenerInterface, DeviceListenerInterface, com.roboclub.robobuggy.ros.Node { private Device thisDevice; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java index 132b2980..ecae33e6 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java @@ -5,27 +5,61 @@ import com.hcrest.jfreespace.Discovery; import com.hcrest.jfreespace.DiscoveryListenerInterface; import com.hcrest.jfreespace.FreespaceErrorCodes; +import com.hcrest.jfreespace.inreport.FreespaceMsgInMotionEngineOutput; import com.hcrest.jfreespace.inreport.HidInMsg; import com.hcrest.jfreespace.outreport.FreespaceMsgOutDataModeControlV2Request; import com.hcrest.jfreespace.outreport.HidOutMsg; import com.roboclub.robobuggy.main.RobobuggyLogicNotification; import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.messages.IMUAccelerationMessage; +import com.roboclub.robobuggy.messages.IMUAngularPositionMessage; +import com.roboclub.robobuggy.messages.IMUAngularVelocityMessage; +import com.roboclub.robobuggy.messages.IMUCompassMessage; +import com.roboclub.robobuggy.messages.IMUInclinationMessage; +import com.roboclub.robobuggy.messages.IMULinearAccelerationMessage; +import com.roboclub.robobuggy.messages.MagneticMeasurement; import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; /** * A node to communicate with the Hillcrest Freespace 9DOF IMU + * + * Comments will make callbacks to page numbers on the datasheet/reference manual, which can be found here: + * http://hillcrestlabs.com/resources/download-materials/download-info/hcomm-reference-manual/ */ +@SuppressWarnings("Duplicates") public class HillcrestNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { private Device hillcrestImu; + // Constants + private static final int IMU_FORMAT_MODE = 1; // Mode 1 - page 21 + + // Publishers for the different messages + private Publisher accelerationPub; + private Publisher linearAccelerationPub; + private Publisher angularVelocityPub; + private Publisher magnetometerPub; + private Publisher inclinationPub; + private Publisher compassHeadingPub; + private Publisher angularPositionPub; + /** * Creates a new Hillcrest IMU node */ public HillcrestNode() { super(new BuggyBaseNode(NodeChannel.IMU), "Hillcrest IMU"); + + // initialize the publishers + accelerationPub = new Publisher(NodeChannel.IMU_ACCELERATION.getMsgPath()); + linearAccelerationPub = new Publisher(NodeChannel.IMU_LINEAR_ACC.getMsgPath()); + angularVelocityPub = new Publisher(NodeChannel.IMU_ANG_VEL.getMsgPath()); + magnetometerPub = new Publisher(NodeChannel.IMU_MAGNETIC.getMsgPath()); + inclinationPub = new Publisher(NodeChannel.IMU_INCLINATION.getMsgPath()); + compassHeadingPub = new Publisher(NodeChannel.IMU_COMPASS.getMsgPath()); + angularPositionPub = new Publisher(NodeChannel.IMU_ANG_POS.getMsgPath()); } @Override @@ -42,11 +76,244 @@ protected boolean shutdownDecoratorNode() { return true; } + /** + * This method parses the IMU data array by using the spec on Pages 22 and 23 of the + * reference manual + * + * @param device the IMU in question + * @param message the message from the IMU + * @param i [Unknown] + * @param l [Unknown] + */ @Override - public void handleRead(Device device, HidInMsg hidInMsg, int i, long l) { + public void handleRead(Device device, HidInMsg message, int i, long l) { + + // first check whether the message was the kind we're looking for + // at this point we are only handling MotionEngineOutput messages + if (!(message instanceof FreespaceMsgInMotionEngineOutput)) { + new RobobuggyLogicNotification("IMU gave us message other than motion engine output!", RobobuggyMessageLevel.NOTE); + return; + } + + FreespaceMsgInMotionEngineOutput imuMotionEngineOutput = ((FreespaceMsgInMotionEngineOutput) message); + + // check whether we are in the correct mode + if (imuMotionEngineOutput.getFormatSelect() != IMU_FORMAT_MODE) { + new RobobuggyLogicNotification("IMU is in an unexpected mode!", RobobuggyMessageLevel.EXCEPTION); + return; + } + + // get the data array, and start our tracker + int[] imuDataArray = imuMotionEngineOutput.getMeData(); + int dataArrayOffset = 0; + + // FF0 measures acceleration in the x, y, z coordinates (outlined on the device) + // x positive is to the right + // y positive is forward + // z positive is up + // reported in units of 0.01g + if (imuMotionEngineOutput.getFf0()) { + + int xAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xAccel = xAccelInHundredths / 100.0; + dataArrayOffset += 2; + + int yAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yAccel = yAccelInHundredths / 100.0; + dataArrayOffset += 2; + + int zAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zAccel = zAccelInHundredths / 100.0; + dataArrayOffset += 2; + + accelerationPub.publish(new IMUAccelerationMessage(xAccel, yAccel, zAccel)); + + } + + // FF1 measures linear acceleration in the xyz plane (outlined on the device) + // x positive is right + // y positive is forward + // z positive is up + // reported in units of 0.01g + if (imuMotionEngineOutput.getFf1()) { + + int xAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xAccel = xAccelInHundredths / 100.0; + dataArrayOffset += 2; + + int yAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yAccel = yAccelInHundredths / 100.0; + dataArrayOffset += 2; + + int zAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zAccel = zAccelInHundredths / 100.0; + dataArrayOffset += 2; + + linearAccelerationPub.publish(new IMULinearAccelerationMessage(xAccel, yAccel, zAccel)); + + } + + // FF2 measures angular velocity + // +x is tilt up (pitch) + // +y is tilt right (roll) + // +z is turn left (yaw) + // reported in units of 0.1 degrees/sec + if (imuMotionEngineOutput.getFf2()) { + + int xAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xAngularVel = xAngularVelocityInTenths / 10.0; + dataArrayOffset += 2; + + int yAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yAngularVel = yAngularVelocityInTenths / 10.0; + dataArrayOffset += 2; + + int zAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zAngularVel = zAngularVelocityInTenths / 10.0; + dataArrayOffset += 2; + + angularVelocityPub.publish(new IMUAngularVelocityMessage(xAngularVel, yAngularVel, zAngularVel)); + + } + + // FF3 measures the magnetometer with respect to the device frame of reference + // +x is forward + // +y is to the right + // +z is down + // reported in units of 0.001 gauss + if (imuMotionEngineOutput.getFf3()) { + + int xMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xMag = xMagInThousandths / 1000.0; + dataArrayOffset += 2; + + int yMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yMag = yMagInThousandths / 1000.0; + dataArrayOffset += 2; + + int zMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zMag = zMagInThousandths / 1000.0; + dataArrayOffset += 2; + + magnetometerPub.publish(new MagneticMeasurement(xMag, yMag, zMag)); + + } + + // FF4 measures the inclination + // +x is tilt up (pitch) + // +y is tilt right (roll) + // +z is turn left (yaw) + // reported in units of 0.1 degrees + if (imuMotionEngineOutput.getFf4()) { + + int xInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xInclination = xInclinationInTenths / 10.0; + dataArrayOffset += 2; + + int yInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yInclination = yInclinationInTenths / 10.0; + dataArrayOffset += 2; + + int zInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zInclination = zInclinationInTenths / 10.0; + dataArrayOffset += 2; + + inclinationPub.publish(new IMUInclinationMessage(xInclination, yInclination, zInclination)); + + } + + // FF5 measures the compass heading + // reported in units of 0.1 degrees + if (imuMotionEngineOutput.getFf5()) { + + int degreesInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double degrees = degreesInTenths/10.0; + dataArrayOffset += 2; + + compassHeadingPub.publish(new IMUCompassMessage(degrees)); + + } + + // FF6 measures angular position + // Gives 4 quaternions (dimensionless) that are expressed as Q14 decimals + if (imuMotionEngineOutput.getFf6()) { + + double w = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); + dataArrayOffset += 2; + double x = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); + dataArrayOffset += 2; + double y = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); + dataArrayOffset += 2; + double z = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); + dataArrayOffset += 2; + + double r11 = 1 - 2 * y * y - 2 * z * z; + double r12 = 2 * x * y - 2 * z * w; + double r13 = 2 * x * z + 2 * y * w; + double r21 = 2 * x * y + 2 * z * w; + double r22 = 1 - 2 * x * x - 2 * z * z; + double r23 = 2 * y * z - 2 * x * w; + double r31 = 2 * x * z - 2 * y * w; + double r32 = 2 * y * z + 2 * x * w; + double r33 = 1 - 2 * x * x - 2 * y * y; + + //TODO normalize + //extracts + double[][] rot = { + { r11, r12, r13 }, + { r21, r22, r23 }, + { r31, r32, r33 } + }; + + angularPositionPub.publish(new IMUAngularPositionMessage(rot)); + + } + + } + + /** + * Returns a 2-byte data value from the IMU's data array + * @param data the byte array for the IMU + * @param offsetInBytes the offset into the data array where the halfword starts from. Note that this value does + * NOT get increased, you will need to keep track of an offset externally + * @return the halfword extracted + */ + protected int extractHalfWordFromDataArray(int[] data, int offsetInBytes) { + + int lsb = data[offsetInBytes]; + int msb = data[offsetInBytes + 1]; + return ((msb << 8) | lsb); + + } + + + /** + * Converts from fixed point to floating point + * + * Some of the imu data is expressed as fixed point numbers, with the format of Q{N}, + * where Q represents 'fixed point', and N represents the point. Q10 represents a + * fixed point number with 1 sign bit, 5 integer bits and 10 fractional bits + * + * More info is found on Page 18 + * + * The algorithm is as follows on Wikipedia: + * https://en.wikipedia.org/wiki/Q_(number_format)#Q_to_float + * + * @param q the 16-bit fixed-point number + * @param n the location of the point + * @return Converted floating point val + */ + protected double convertQNToDouble(int q, int n) { + + // convert it to a double + double qDouble = (double) q; + + // multiply it by 2^(-n) + return qDouble * Math.pow(2, -1 * n); } + @Override public void handleSend(Device device, HidOutMsg hidOutMsg, int i) { @@ -62,16 +329,18 @@ public void freespaceDeviceInserted(Device device) { hillcrestImu = device; hillcrestImu.open(this); - int modeAndStatus = 8; // bit 3 set gives Full Motion On mode - page 41 - int packetSelect = 8; // 8 = MotionEngineOutput - page 41 - int formatSelect = 1; // Mode 1 - page 18 + int modeAndStatus = 8; // bit 3 set gives Full Motion On mode - page 41 + int packetSelect = 8; // 8 = MotionEngineOutput - page 41 + int formatSelect = IMU_FORMAT_MODE; // Mode 1 - page 21 + FreespaceMsgOutDataModeControlV2Request configMsg = new FreespaceMsgOutDataModeControlV2Request(); configMsg.setModeAndStatus(modeAndStatus); configMsg.setPacketSelect(packetSelect); configMsg.setFormatSelect(formatSelect); - // get all the data + // we want to make sure that we get all the packets + // the Format Flags are basically a way to enable each individual sensor - page 41 configMsg.setFf0(true); configMsg.setFf1(true); configMsg.setFf2(true); @@ -81,6 +350,7 @@ public void freespaceDeviceInserted(Device device) { configMsg.setFf6(true); configMsg.setFf7(true); + // send down the config info, and get the response FreespaceErrorCodes response = hillcrestImu.sendMessage(configMsg); if (!response.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { new RobobuggyLogicNotification("Error configuring IMU!", RobobuggyMessageLevel.EXCEPTION); diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java index 76fff61a..326d0c32 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java @@ -11,11 +11,14 @@ */ public class HillcrestNodeTest { + private HillcrestNode testNode; + /** * setup done before each test */ @Before public void setUp() { + testNode = new HillcrestNode(); } /** @@ -23,7 +26,6 @@ public void setUp() { */ @After public void tearDown() { - } /** @@ -38,4 +40,61 @@ public void testCreation() { assertEquals(imu.getName(), "Hillcrest IMU"); } + /** + * Tests that we can convert fixed point numbers to float + * + * Inputs a set of fixed point numbers expressed as integers + * Expects the "correctly" converted numbers - the fixed point system + * has a resolution of 2^(-n) + * + * No errors should be thrown + */ + @Test + public void testConvertQn() { + int q1 = 53; + int n1 = 2; + double expected1 = 13.37; + assertEquals(expected1, testNode.convertQNToDouble(q1, n1), 0.25); + + int q2 = 12868; + int n2 = 12; + double expected2 = 3.1415; + assertEquals(expected2, testNode.convertQNToDouble(q2, n2), 0.0002); + + int q3 = 87; + int n3 = 5; + double expected3 = 2.7181; + assertEquals(expected3, testNode.convertQNToDouble(q3, n3), 0.0312); + + int q4 = 69388; + int n4 = 14; + double expected4 = 4.2351; + assertEquals(expected4, testNode.convertQNToDouble(q4, n4), 0.00006); + + int q5 = 3900; + int n5 = 8; + double expected5 = 15.2352; + assertEquals(expected5, testNode.convertQNToDouble(q5, n5), 0.0039); + + int q6 = 0; + int n6 = 10; + double expected6 = 0.0; + assertEquals(expected6, testNode.convertQNToDouble(q6, n6), 0.0); + + // int max for q14 + int q7 = 0x7FFF; + int n7 = 1; + double expected7 = 16383.5; + assertEquals(expected7, testNode.convertQNToDouble(q7, n7), 0.0); + + // int min for q14 + int q8 = 0x8000; + int n8 = 1; + double expected8 = -16384.0; + assertEquals(expected8, testNode.convertQNToDouble(q8, n8), 0.0); + + + } + + } \ No newline at end of file From f4398dba2f16418e34691edc3ab37b28025e6949 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 22:47:31 -0500 Subject: [PATCH 07/25] corrected sign issues --- .../robobuggy/nodes/sensors/HillcrestNode.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java index ecae33e6..b92027f3 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java @@ -308,8 +308,18 @@ protected double convertQNToDouble(int q, int n) { // convert it to a double double qDouble = (double) q; + // get the sign bit + int signBit = (q >> 15) & 0x1; + int sign = 0; + if (signBit == 0) { + sign = 1; + } + else { + sign = -1; + } + // multiply it by 2^(-n) - return qDouble * Math.pow(2, -1 * n); + return sign * qDouble * Math.pow(2, -1 * n); } From bf8949523708f91eb9ff1ca160265700dc5b9b00 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 22:55:38 -0500 Subject: [PATCH 08/25] took out old hillcrest node --- .../nodes/sensors/HillCrestImuNode.java | 297 ------------------ .../nodes/sensors/HillcrestNode.java | 1 - .../robobuggy/robots/TransistorAuton.java | 1 - 3 files changed, 299 deletions(-) delete mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java deleted file mode 100644 index 425493ea..00000000 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java +++ /dev/null @@ -1,297 +0,0 @@ -package com.roboclub.robobuggy.nodes.sensors; - -import com.hcrest.jfreespace.Device; -import com.hcrest.jfreespace.DeviceListenerInterface; -import com.hcrest.jfreespace.Discovery; -import com.hcrest.jfreespace.DiscoveryListenerInterface; -import com.hcrest.jfreespace.inreport.FreespaceMsgInMotionEngineOutput; -import com.hcrest.jfreespace.inreport.HidInMsg; -import com.hcrest.jfreespace.outreport.FreespaceMsgOutDataModeControlV2Request; -import com.hcrest.jfreespace.outreport.HidOutMsg; -import com.roboclub.robobuggy.messages.IMUAccelerationMessage; -import com.roboclub.robobuggy.messages.IMUAngularPositionMessage; -import com.roboclub.robobuggy.messages.IMUAngularVelocityMessage; -import com.roboclub.robobuggy.messages.IMUInclinationMessage; -import com.roboclub.robobuggy.messages.IMULinearAccelerationMessage; -import com.roboclub.robobuggy.messages.IMUCompassMessage; -import com.roboclub.robobuggy.messages.MagneticMeasurement; -import com.roboclub.robobuggy.ros.NodeChannel; -import com.roboclub.robobuggy.ros.Publisher; - -/** - * Driver for the Hillcrest IMUs - * - * @author Trevor Decker - * @author Sean Buckley - */ -@Deprecated -@SuppressWarnings("Duplicates") -public class HillCrestImuNode implements DiscoveryListenerInterface, DeviceListenerInterface, com.roboclub.robobuggy.ros.Node { - private Device thisDevice; - - private Publisher accelPub = new Publisher(NodeChannel.IMU_ACCELERATION.getMsgPath()); - private Publisher linearAccPub = new Publisher(NodeChannel.IMU_LINEAR_ACC.getMsgPath()); - private Publisher inclinationPub = new Publisher(NodeChannel.IMU_INCLINATION.getMsgPath()); - private Publisher angVelPub = new Publisher(NodeChannel.IMU_ANG_VEL.getMsgPath()); - private Publisher magPub = new Publisher(NodeChannel.IMU_MAGNETIC.getMsgPath()); - private Publisher compassPub = new Publisher(NodeChannel.IMU_COMPASS.getMsgPath()); - private Publisher angPosPub = new Publisher(NodeChannel.IMU_ANG_POS.getMsgPath()); - - /** - * Constructor for the hillcrest imu - */ - public HillCrestImuNode() { - super(); - Discovery discover = Discovery.getInstance(); - discover.addListener(this); - - } - - - @Override - public void handleRead(Device arg0, HidInMsg arg1, int arg2, long arg3) { - //TODO handle other kinds of messages - if (!(arg1 instanceof FreespaceMsgInMotionEngineOutput)) { - return; - } - - FreespaceMsgInMotionEngineOutput m = (FreespaceMsgInMotionEngineOutput) arg1; - - // we can only parse format 1 for now - if (m.getFormatSelect() != 1) { - return; - } - - int[] data = m.getMeData(); - int offsetInBytes = 0; - - // FF0 is acceleration for Format 1 - // reported in units of 0.01g - if (m.getFf0()) { - int xAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); - double xAccel = xAccelInHundredths / 100.0; - offsetInBytes += 2; - - int yAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); - double yAccel = yAccelInHundredths / 100.0; - offsetInBytes += 2; - - int zAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); - double zAccel = zAccelInHundredths / 100.0; - offsetInBytes += 2; - - accelPub.publish(new IMUAccelerationMessage(xAccel, yAccel, zAccel)); - - } - //ff1 is linear acceleration for Format 1 - // reported in units of 0.01g - if (m.getFf1()) { - int xAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); - double xAccel = xAccelInHundredths / 100.0; - offsetInBytes += 2; - - int yAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); - double yAccel = yAccelInHundredths / 100.0; - offsetInBytes += 2; - - int zAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); - double zAccel = zAccelInHundredths / 100.0; - offsetInBytes += 2; - - linearAccPub.publish(new IMULinearAccelerationMessage(xAccel, yAccel, zAccel)); - } - //ff2 is angular velocity for Format 1 - // reported in units of 0.1 deg/sec - if (m.getFf2()) { - int xAngularVelocityInTenths = extractHalfWordFromDataArray(data, offsetInBytes); - double xAngularVel = xAngularVelocityInTenths / 10.0; - offsetInBytes += 2; - - int yAngularVelocityInTenths = extractHalfWordFromDataArray(data, offsetInBytes); - double yAngularVel = yAngularVelocityInTenths / 10.0; - offsetInBytes += 2; - - int zAngularVelocityInTenths = extractHalfWordFromDataArray(data, offsetInBytes); - double zAngularVel = zAngularVelocityInTenths / 10.0; - offsetInBytes += 2; - - angVelPub.publish(new IMUAngularVelocityMessage(xAngularVel, yAngularVel, zAngularVel)); - } - //ff3 is magnetometer for Format 1 - // reported in units of 0.001 gauss - if (m.getFf3()) { - int xMagInThousandths = extractHalfWordFromDataArray(data, offsetInBytes); - double xMag = xMagInThousandths / 1000.0; - offsetInBytes += 2; - - int yMagInThousandths = extractHalfWordFromDataArray(data, offsetInBytes); - double yMag = yMagInThousandths / 1000.0; - offsetInBytes += 2; - - int zMagInThousandths = extractHalfWordFromDataArray(data, offsetInBytes); - double zMag = zMagInThousandths / 1000.0; - offsetInBytes += 2; - - magPub.publish(new MagneticMeasurement(xMag, yMag, zMag)); - } - //ff4 is inclination - // reported in units of 0.1 degrees - if (m.getFf4()) { - int xInclinationInTenths = extractHalfWordFromDataArray(data, offsetInBytes); - double xInclination = xInclinationInTenths / 10.0; - offsetInBytes += 2; - - int yInclinationInTenths = extractHalfWordFromDataArray(data, offsetInBytes); - double yInclination = yInclinationInTenths / 10.0; - offsetInBytes += 2; - - int zInclinationInTenths = extractHalfWordFromDataArray(data, offsetInBytes); - double zInclination = zInclinationInTenths / 10.0; - offsetInBytes += 2; - - inclinationPub.publish(new IMUInclinationMessage(xInclination, yInclination, zInclination)); - } - //ff5 is compass for format 1 - if (m.getFf5()) { - int degreesInTenths = extractHalfWordFromDataArray(data, offsetInBytes); - double degrees = degreesInTenths/10.0; - offsetInBytes += 2; - - compassPub.publish(new IMUCompassMessage(degrees)); - } - //ff6 is angular position - if (m.getFf6()) { - double w = convertQNToDouble((byte) data[offsetInBytes + 0], (byte) data[offsetInBytes + 1], 14); - double x = convertQNToDouble((byte) data[offsetInBytes + 2], (byte) data[offsetInBytes + 3], 14); - double y = convertQNToDouble((byte) data[offsetInBytes + 4], (byte) data[offsetInBytes + 5], 14); - double z = convertQNToDouble((byte) data[offsetInBytes + 6], (byte) data[offsetInBytes + 7], 14); - - double r11 = 1 - 2 * y * y - 2 * z * z; - double r12 = 2 * x * y - 2 * z * w; - double r13 = 2 * x * z + 2 * y * w; - double r21 = 2 * x * y + 2 * z * w; - double r22 = 1 - 2 * x * x - 2 * z * z; - double r23 = 2 * y * z - 2 * x * w; - double r31 = 2 * x * z - 2 * y * w; - double r32 = 2 * y * z + 2 * x * w; - double r33 = 1 - 2 * x * x - 2 * y * y; - - //TODO normalize - //extracts - double[][] rot = { - { r11, r12, r13 }, - { r21, r22, r23 }, - { r31, r32, r33 } - }; - - angPosPub.publish(new IMUAngularPositionMessage(rot)); - offsetInBytes += 8; - } - - } - - @Override - public void handleSend(Device arg0, HidOutMsg arg1, int arg2) { - // TODO Auto-generated method stub - - } - - @Override - public void notifyRemoved(Device arg0) { - // TODO Auto-generated method stub - - } - - @Override - public void freespaceDeviceInserted(Device arg0) { - // TODO Auto-generated method stub - thisDevice = arg0; - thisDevice.open(this); - - FreespaceMsgOutDataModeControlV2Request msg = new FreespaceMsgOutDataModeControlV2Request(); - msg.setPacketSelect(8); // - msg.setModeAndStatus(8); - //Set to 0 for gyro, accel, temp, mag, ang data. Set to 1 for compass - msg.setFormatSelect(1); - //we really don't know what the format is so we are just going to log everything for now - msg.setFf2(true); //linear Acceleration without Gravity - msg.setFf3(true); //Angular Velocity - msg.setFf4(true); //Magnetometer - msg.setFf5(true); //Tempeature - msg.setFf6(true); //Angular position - - thisDevice.sendMessageAsync(msg); - } - - @Override - public void freespaceDeviceRemoved(Device arg0) { - // TODO Auto-generated method stub - thisDevice = null; - - } - - @Override - public boolean startNode() { - // TODO Auto-generated method stub - return true; - } - - @Override - public boolean shutdown() { - // TODO Auto-generated method stub - return false; - } - - @Override - public void setName(String newName) { - // TODO Auto-generated method stub - - } - - @Override - public String getName() { - // TODO Auto-generated method stub - return "hillcrestImu"; - } - - /** - * Func: Converts from fixed point to floating point - * - * @param lsb least significant byte - * @param msb most significant byte - * @param qn number of fraction bits - * @return Converted floating point val - */ - public double convertQNToDouble(byte lsb, byte msb, int qn) { - // converting Qn fixed point - - int msbAsInt = ((int) msb) & 0xFF; - int lsbAsInt = ((int) lsb & 0xFF); - - int squashedTogether = ((msbAsInt << 8) | lsbAsInt); - Integer sT = Integer.valueOf(squashedTogether); - - if (sT >= 1 << 15) { - sT = sT - (1 << 17 - 1); - } - - double d = sT.doubleValue(); - return Math.pow(2, -1 * qn) * d; - - } - - /** - * Returns a 2-byte data value from the IMU's data array - * @param data the byte array for the IMU - * @param offsetInBytes the offset into the data array where the halfword starts from. Note that this value does - * NOT get increased, you will need to keep track of an offset externally - * @return the halfword extracted - */ - private int extractHalfWordFromDataArray(int[] data, int offsetInBytes) { - - int lsb = data[offsetInBytes]; - int msb = data[offsetInBytes + 1]; - return ((msb << 8) | lsb); - } - -} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java index b92027f3..f61287c5 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java @@ -29,7 +29,6 @@ * Comments will make callbacks to page numbers on the datasheet/reference manual, which can be found here: * http://hillcrestlabs.com/resources/download-materials/download-info/hcomm-reference-manual/ */ -@SuppressWarnings("Duplicates") public class HillcrestNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { private Device hillcrestImu; diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 751b602e..205ffc32 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -8,7 +8,6 @@ import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; From 634724eb3d2aba226aa378cb8427fe1980cf8ab2 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 23:05:45 -0500 Subject: [PATCH 09/25] fixed findbugs issues --- .../{HillcrestNode.java => HillcrestImuNode.java} | 4 ++-- .../com/roboclub/robobuggy/robots/TransistorAuton.java | 3 ++- .../robobuggy/robots/TransistorDataCollection.java | 4 ++-- ...illcrestNodeTest.java => HillcrestImuNodeTest.java} | 10 +++++----- 4 files changed, 11 insertions(+), 10 deletions(-) rename real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/{HillcrestNode.java => HillcrestImuNode.java} (98%) rename real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/{HillcrestNodeTest.java => HillcrestImuNodeTest.java} (91%) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java similarity index 98% rename from real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java rename to real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java index f61287c5..8d12e9fa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java @@ -29,7 +29,7 @@ * Comments will make callbacks to page numbers on the datasheet/reference manual, which can be found here: * http://hillcrestlabs.com/resources/download-materials/download-info/hcomm-reference-manual/ */ -public class HillcrestNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { +public class HillcrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { private Device hillcrestImu; @@ -48,7 +48,7 @@ public class HillcrestNode extends BuggyDecoratorNode implements DeviceListenerI /** * Creates a new Hillcrest IMU node */ - public HillcrestNode() { + public HillcrestImuNode() { super(new BuggyBaseNode(NodeChannel.IMU), "Hillcrest IMU"); // initialize the publishers diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 205ffc32..f51d9420 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -8,6 +8,7 @@ import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.GpsNode; +import com.roboclub.robobuggy.nodes.sensors.HillcrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -66,7 +67,7 @@ private TransistorAuton() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); // nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillCrestImuNode()); + nodeList.add(new HillcrestImuNode()); try { ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java index 2980405d..9e6e4e9e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java @@ -5,7 +5,7 @@ import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.nodes.sensors.CameraNode; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; +import com.roboclub.robobuggy.nodes.sensors.HillcrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -56,7 +56,7 @@ private TransistorDataCollection() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillCrestImuNode()); + nodeList.add(new HillcrestImuNode()); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java similarity index 91% rename from real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java rename to real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java index 326d0c32..104d4aa2 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestNodeTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java @@ -9,16 +9,16 @@ /** * Created by vivaanbahl on 9/8/16. */ -public class HillcrestNodeTest { +public class HillcrestImuNodeTest { - private HillcrestNode testNode; + private HillcrestImuNode testNode; /** * setup done before each test */ @Before public void setUp() { - testNode = new HillcrestNode(); + testNode = new HillcrestImuNode(); } /** @@ -36,7 +36,7 @@ public void tearDown() { */ @Test public void testCreation() { - HillcrestNode imu = new HillcrestNode(); + HillcrestImuNode imu = new HillcrestImuNode(); assertEquals(imu.getName(), "Hillcrest IMU"); } @@ -58,7 +58,7 @@ public void testConvertQn() { int q2 = 12868; int n2 = 12; - double expected2 = 3.1415; + double expected2 = 3.1417; assertEquals(expected2, testNode.convertQNToDouble(q2, n2), 0.0002); int q3 = 87; From 128a812620c52bb5f433341b71b79820e40f2f83 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 23:07:22 -0500 Subject: [PATCH 10/25] renamed the new class to fit the old one so that we can compare diffs rather than full classes --- .../robobuggy/nodes/sensors/HillcrestImuNode.java | 4 ++-- .../com/roboclub/robobuggy/robots/TransistorAuton.java | 4 ++-- .../robobuggy/robots/TransistorDataCollection.java | 4 ++-- .../robobuggy/nodes/sensors/HillcrestImuNodeTest.java | 8 ++++---- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java index 8d12e9fa..fc581289 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java @@ -29,7 +29,7 @@ * Comments will make callbacks to page numbers on the datasheet/reference manual, which can be found here: * http://hillcrestlabs.com/resources/download-materials/download-info/hcomm-reference-manual/ */ -public class HillcrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { +public class HillCrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { private Device hillcrestImu; @@ -48,7 +48,7 @@ public class HillcrestImuNode extends BuggyDecoratorNode implements DeviceListen /** * Creates a new Hillcrest IMU node */ - public HillcrestImuNode() { + public HillCrestImuNode() { super(new BuggyBaseNode(NodeChannel.IMU), "Hillcrest IMU"); // initialize the publishers diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index f51d9420..751b602e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -8,7 +8,7 @@ import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillcrestImuNode; +import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -67,7 +67,7 @@ private TransistorAuton() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); // nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillcrestImuNode()); + nodeList.add(new HillCrestImuNode()); try { ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java index 9e6e4e9e..2980405d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java @@ -5,7 +5,7 @@ import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.nodes.sensors.CameraNode; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillcrestImuNode; +import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -56,7 +56,7 @@ private TransistorDataCollection() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillcrestImuNode()); + nodeList.add(new HillCrestImuNode()); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java index 104d4aa2..7e51a998 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java @@ -9,16 +9,16 @@ /** * Created by vivaanbahl on 9/8/16. */ -public class HillcrestImuNodeTest { +public class HillCrestImuNodeTest { - private HillcrestImuNode testNode; + private HillCrestImuNode testNode; /** * setup done before each test */ @Before public void setUp() { - testNode = new HillcrestImuNode(); + testNode = new HillCrestImuNode(); } /** @@ -36,7 +36,7 @@ public void tearDown() { */ @Test public void testCreation() { - HillcrestImuNode imu = new HillcrestImuNode(); + HillCrestImuNode imu = new HillCrestImuNode(); assertEquals(imu.getName(), "Hillcrest IMU"); } From 9ce77c5194805116fb98b7ce838c18c24fa62842 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 23:19:30 -0500 Subject: [PATCH 11/25] added gradle clean to travis --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 16ddfbb3..8e8693d8 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,7 +20,7 @@ script: - echo $HOME - echo $BUGGY_ROOT # for running the build and tests of the high level code - - (cd real_time/surface_src/java_src/Alice && $GRADLE_BIN/gradle build) + - (cd real_time/surface_src/java_src/Alice && $GRADLE_BIN/gradle clean && $GRADLE_BIN/gradle build) - bash real_time/surface_src/java_src/Alice/travisStyleCheck.sh - bash real_time/arduino_src/travisStyleCheck.sh From 3ad887a5c3ce46dd5854b5cdb03a753a0de2faa6 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 23:23:16 -0500 Subject: [PATCH 12/25] never mind --- .../robobuggy/nodes/sensors/HillcrestImuNode.java | 4 ++-- .../com/roboclub/robobuggy/robots/TransistorAuton.java | 4 ++-- .../robobuggy/robots/TransistorDataCollection.java | 4 ++-- .../robobuggy/nodes/sensors/HillcrestImuNodeTest.java | 8 ++++---- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java index fc581289..8d12e9fa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java @@ -29,7 +29,7 @@ * Comments will make callbacks to page numbers on the datasheet/reference manual, which can be found here: * http://hillcrestlabs.com/resources/download-materials/download-info/hcomm-reference-manual/ */ -public class HillCrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { +public class HillcrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { private Device hillcrestImu; @@ -48,7 +48,7 @@ public class HillCrestImuNode extends BuggyDecoratorNode implements DeviceListen /** * Creates a new Hillcrest IMU node */ - public HillCrestImuNode() { + public HillcrestImuNode() { super(new BuggyBaseNode(NodeChannel.IMU), "Hillcrest IMU"); // initialize the publishers diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index 751b602e..f51d9420 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -8,7 +8,7 @@ import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; +import com.roboclub.robobuggy.nodes.sensors.HillcrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -67,7 +67,7 @@ private TransistorAuton() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); // nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillCrestImuNode()); + nodeList.add(new HillcrestImuNode()); try { ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java index 2980405d..9e6e4e9e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java @@ -5,7 +5,7 @@ import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.nodes.sensors.CameraNode; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; +import com.roboclub.robobuggy.nodes.sensors.HillcrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -56,7 +56,7 @@ private TransistorDataCollection() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillCrestImuNode()); + nodeList.add(new HillcrestImuNode()); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java index 7e51a998..104d4aa2 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java @@ -9,16 +9,16 @@ /** * Created by vivaanbahl on 9/8/16. */ -public class HillCrestImuNodeTest { +public class HillcrestImuNodeTest { - private HillCrestImuNode testNode; + private HillcrestImuNode testNode; /** * setup done before each test */ @Before public void setUp() { - testNode = new HillCrestImuNode(); + testNode = new HillcrestImuNode(); } /** @@ -36,7 +36,7 @@ public void tearDown() { */ @Test public void testCreation() { - HillCrestImuNode imu = new HillCrestImuNode(); + HillcrestImuNode imu = new HillcrestImuNode(); assertEquals(imu.getName(), "Hillcrest IMU"); } From b43aa4640a9053860e547bc57364abf477ec89e2 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Mon, 21 Nov 2016 23:25:28 -0500 Subject: [PATCH 13/25] forgot about this --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 8e8693d8..16ddfbb3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,7 +20,7 @@ script: - echo $HOME - echo $BUGGY_ROOT # for running the build and tests of the high level code - - (cd real_time/surface_src/java_src/Alice && $GRADLE_BIN/gradle clean && $GRADLE_BIN/gradle build) + - (cd real_time/surface_src/java_src/Alice && $GRADLE_BIN/gradle build) - bash real_time/surface_src/java_src/Alice/travisStyleCheck.sh - bash real_time/arduino_src/travisStyleCheck.sh From 20e2b846960bc0eceb76bda568121ac96e428375 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 22 Nov 2016 13:06:06 -0500 Subject: [PATCH 14/25] added error checking to the open --- .../robobuggy/nodes/sensors/HillcrestImuNode.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java index 8d12e9fa..bf481f5b 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java @@ -336,7 +336,13 @@ public void notifyRemoved(Device device) { @Override public void freespaceDeviceInserted(Device device) { hillcrestImu = device; - hillcrestImu.open(this); + + // open the device + FreespaceErrorCodes openResponse = hillcrestImu.open(this); + if (!openResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { + new RobobuggyLogicNotification("Error opening IMU!", RobobuggyMessageLevel.EXCEPTION); + return; + } int modeAndStatus = 8; // bit 3 set gives Full Motion On mode - page 41 int packetSelect = 8; // 8 = MotionEngineOutput - page 41 @@ -360,8 +366,8 @@ public void freespaceDeviceInserted(Device device) { configMsg.setFf7(true); // send down the config info, and get the response - FreespaceErrorCodes response = hillcrestImu.sendMessage(configMsg); - if (!response.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { + FreespaceErrorCodes configResponse = hillcrestImu.sendMessage(configMsg); + if (!configResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { new RobobuggyLogicNotification("Error configuring IMU!", RobobuggyMessageLevel.EXCEPTION); } } From 2c636cb29be51df7cb1639a7ffa21a61756185e2 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 22 Nov 2016 21:25:54 -0500 Subject: [PATCH 15/25] added error checking to the device close --- .../roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java index bf481f5b..ee256a8b 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java @@ -374,7 +374,10 @@ public void freespaceDeviceInserted(Device device) { @Override public void freespaceDeviceRemoved(Device device) { - hillcrestImu.close(); + FreespaceErrorCodes closeResponse = hillcrestImu.close(); + if (!closeResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { + new RobobuggyLogicNotification("Error closing IMU device!", RobobuggyMessageLevel.EXCEPTION); + } hillcrestImu = null; } } From c163b0321c46176f02d9cfc19290d5d8127ef3af Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 25 Nov 2016 11:24:18 -0500 Subject: [PATCH 16/25] oops didn't realize it was a void --- .../roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java index ee256a8b..17fab380 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java @@ -374,10 +374,8 @@ public void freespaceDeviceInserted(Device device) { @Override public void freespaceDeviceRemoved(Device device) { - FreespaceErrorCodes closeResponse = hillcrestImu.close(); - if (!closeResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { - new RobobuggyLogicNotification("Error closing IMU device!", RobobuggyMessageLevel.EXCEPTION); - } + // close doesn't return a status + hillcrestImu.close(); hillcrestImu = null; } } From fc3b2618e004f43dc9177263a4c23525170853a8 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 29 Nov 2016 01:11:01 -0500 Subject: [PATCH 17/25] trying this again hopefully won't fail travis --- .../nodes/sensors/HillcrestImuNode.java | 4 +-- .../robobuggy/robots/TransistorAuton.java | 4 +-- .../robots/TransistorDataCollection.java | 4 +-- .../src/test/java/TestImuMeasurement.java | 36 ------------------- .../nodes/sensors/HillcrestImuNodeTest.java | 8 ++--- 5 files changed, 10 insertions(+), 46 deletions(-) delete mode 100644 real_time/surface_src/java_src/Alice/src/test/java/TestImuMeasurement.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java index 17fab380..c3dc18a8 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java @@ -29,7 +29,7 @@ * Comments will make callbacks to page numbers on the datasheet/reference manual, which can be found here: * http://hillcrestlabs.com/resources/download-materials/download-info/hcomm-reference-manual/ */ -public class HillcrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { +public class HillCrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { private Device hillcrestImu; @@ -48,7 +48,7 @@ public class HillcrestImuNode extends BuggyDecoratorNode implements DeviceListen /** * Creates a new Hillcrest IMU node */ - public HillcrestImuNode() { + public HillCrestImuNode() { super(new BuggyBaseNode(NodeChannel.IMU), "Hillcrest IMU"); // initialize the publishers diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java index f51d9420..751b602e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorAuton.java @@ -8,7 +8,7 @@ import com.roboclub.robobuggy.nodes.planners.WayPointFollowerPlanner; import com.roboclub.robobuggy.nodes.planners.WayPointUtil; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillcrestImuNode; +import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -67,7 +67,7 @@ private TransistorAuton() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); // nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillcrestImuNode()); + nodeList.add(new HillCrestImuNode()); try { ArrayList wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java index 9e6e4e9e..2980405d 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/TransistorDataCollection.java @@ -5,7 +5,7 @@ import com.roboclub.robobuggy.main.RobobuggyMessageLevel; import com.roboclub.robobuggy.nodes.sensors.CameraNode; import com.roboclub.robobuggy.nodes.sensors.GpsNode; -import com.roboclub.robobuggy.nodes.sensors.HillcrestImuNode; +import com.roboclub.robobuggy.nodes.sensors.HillCrestImuNode; import com.roboclub.robobuggy.nodes.sensors.LoggingNode; import com.roboclub.robobuggy.nodes.sensors.RBSMNode; import com.roboclub.robobuggy.ros.NodeChannel; @@ -56,7 +56,7 @@ private TransistorDataCollection() { nodeList.add(new RBSMNode(NodeChannel.ENCODER, NodeChannel.STEERING, RobobuggyConfigFile.getComPortRBSM(), RobobuggyConfigFile.RBSM_COMMAND_PERIOD)); nodeList.add(new CameraNode(NodeChannel.PUSHBAR_CAMERA, 100)); - nodeList.add(new HillcrestImuNode()); + nodeList.add(new HillCrestImuNode()); //setup the gui RobobuggyJFrame mainWindow = new RobobuggyJFrame("MainWindow", 1.0, 1.0); diff --git a/real_time/surface_src/java_src/Alice/src/test/java/TestImuMeasurement.java b/real_time/surface_src/java_src/Alice/src/test/java/TestImuMeasurement.java deleted file mode 100644 index 370bc763..00000000 --- a/real_time/surface_src/java_src/Alice/src/test/java/TestImuMeasurement.java +++ /dev/null @@ -1,36 +0,0 @@ -import com.roboclub.robobuggy.messages.ImuMeasurement; -import org.junit.Test; - -import static org.junit.Assert.fail; - - -/** - * tests the imu's measurement mathematics - */ -public class TestImuMeasurement { - - /** - * tests the imu's math - */ - @Test - public void test() { - double roll = 0.123; - double pitch = 4.567; - double yaw = 8.9; - ImuMeasurement aMeasurement = new ImuMeasurement(yaw, pitch, roll); - if (Math.abs(aMeasurement.getPitch() - pitch) > .0001) { - fail("pitch does not equal pitch"); - } - - if (Math.abs(aMeasurement.getRoll() - roll) > .0001) { - fail("roll does not equal roll"); - } - - if (Math.abs(aMeasurement.getYaw() - yaw) > .0001) { - fail("yaw does not equal yaw"); - } - - - } - -} diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java index 104d4aa2..7e51a998 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java @@ -9,16 +9,16 @@ /** * Created by vivaanbahl on 9/8/16. */ -public class HillcrestImuNodeTest { +public class HillCrestImuNodeTest { - private HillcrestImuNode testNode; + private HillCrestImuNode testNode; /** * setup done before each test */ @Before public void setUp() { - testNode = new HillcrestImuNode(); + testNode = new HillCrestImuNode(); } /** @@ -36,7 +36,7 @@ public void tearDown() { */ @Test public void testCreation() { - HillcrestImuNode imu = new HillcrestImuNode(); + HillCrestImuNode imu = new HillCrestImuNode(); assertEquals(imu.getName(), "Hillcrest IMU"); } From f00e9a4bfd866df20dc29681f0f5228fcdb99cb8 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 29 Nov 2016 01:20:16 -0500 Subject: [PATCH 18/25] I think I fixed it? Git didn't track the renamed file but my computer did --- .gitignore | 4 ++++ .../sensors/HillcrestImuNode.java => HillCrestImuNode.java} | 0 2 files changed, 4 insertions(+) rename real_time/surface_src/java_src/{Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java => HillCrestImuNode.java} (100%) diff --git a/.gitignore b/.gitignore index 2406451b..b2e14832 100644 --- a/.gitignore +++ b/.gitignore @@ -40,3 +40,7 @@ cwthbntv.nci.txt *.iml real_time/surface_src/java_src/Alice/test.mp4 t30euete.f4z.txt +bytes from arduino.txt +other.txt +real_time/surface_src/java_src/Alice/.idea +real_time/surface_src/java_src/Alice/library/protobuf-3.0.0 diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java b/real_time/surface_src/java_src/HillCrestImuNode.java similarity index 100% rename from real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNode.java rename to real_time/surface_src/java_src/HillCrestImuNode.java From 2aef681295e7e6bb177698f7538d354f8c6c74f1 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 29 Nov 2016 01:23:08 -0500 Subject: [PATCH 19/25] dangerous territory here --- .../nodes/sensors/HillCrestImuNode.java | 381 ++++++++++++++++++ 1 file changed, 381 insertions(+) create mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java new file mode 100644 index 00000000..c3dc18a8 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java @@ -0,0 +1,381 @@ +package com.roboclub.robobuggy.nodes.sensors; + +import com.hcrest.jfreespace.Device; +import com.hcrest.jfreespace.DeviceListenerInterface; +import com.hcrest.jfreespace.Discovery; +import com.hcrest.jfreespace.DiscoveryListenerInterface; +import com.hcrest.jfreespace.FreespaceErrorCodes; +import com.hcrest.jfreespace.inreport.FreespaceMsgInMotionEngineOutput; +import com.hcrest.jfreespace.inreport.HidInMsg; +import com.hcrest.jfreespace.outreport.FreespaceMsgOutDataModeControlV2Request; +import com.hcrest.jfreespace.outreport.HidOutMsg; +import com.roboclub.robobuggy.main.RobobuggyLogicNotification; +import com.roboclub.robobuggy.main.RobobuggyMessageLevel; +import com.roboclub.robobuggy.messages.IMUAccelerationMessage; +import com.roboclub.robobuggy.messages.IMUAngularPositionMessage; +import com.roboclub.robobuggy.messages.IMUAngularVelocityMessage; +import com.roboclub.robobuggy.messages.IMUCompassMessage; +import com.roboclub.robobuggy.messages.IMUInclinationMessage; +import com.roboclub.robobuggy.messages.IMULinearAccelerationMessage; +import com.roboclub.robobuggy.messages.MagneticMeasurement; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; +import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; +import com.roboclub.robobuggy.ros.NodeChannel; +import com.roboclub.robobuggy.ros.Publisher; + +/** + * A node to communicate with the Hillcrest Freespace 9DOF IMU + * + * Comments will make callbacks to page numbers on the datasheet/reference manual, which can be found here: + * http://hillcrestlabs.com/resources/download-materials/download-info/hcomm-reference-manual/ + */ +public class HillCrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { + + private Device hillcrestImu; + + // Constants + private static final int IMU_FORMAT_MODE = 1; // Mode 1 - page 21 + + // Publishers for the different messages + private Publisher accelerationPub; + private Publisher linearAccelerationPub; + private Publisher angularVelocityPub; + private Publisher magnetometerPub; + private Publisher inclinationPub; + private Publisher compassHeadingPub; + private Publisher angularPositionPub; + + /** + * Creates a new Hillcrest IMU node + */ + public HillCrestImuNode() { + super(new BuggyBaseNode(NodeChannel.IMU), "Hillcrest IMU"); + + // initialize the publishers + accelerationPub = new Publisher(NodeChannel.IMU_ACCELERATION.getMsgPath()); + linearAccelerationPub = new Publisher(NodeChannel.IMU_LINEAR_ACC.getMsgPath()); + angularVelocityPub = new Publisher(NodeChannel.IMU_ANG_VEL.getMsgPath()); + magnetometerPub = new Publisher(NodeChannel.IMU_MAGNETIC.getMsgPath()); + inclinationPub = new Publisher(NodeChannel.IMU_INCLINATION.getMsgPath()); + compassHeadingPub = new Publisher(NodeChannel.IMU_COMPASS.getMsgPath()); + angularPositionPub = new Publisher(NodeChannel.IMU_ANG_POS.getMsgPath()); + } + + @Override + protected boolean startDecoratorNode() { + // look for an IMU, and add this as a listener + Discovery.getInstance().addListener(this); + + return true; + } + + @Override + protected boolean shutdownDecoratorNode() { + Discovery.getInstance().removeListener(this); + return true; + } + + /** + * This method parses the IMU data array by using the spec on Pages 22 and 23 of the + * reference manual + * + * @param device the IMU in question + * @param message the message from the IMU + * @param i [Unknown] + * @param l [Unknown] + */ + @Override + public void handleRead(Device device, HidInMsg message, int i, long l) { + + // first check whether the message was the kind we're looking for + // at this point we are only handling MotionEngineOutput messages + if (!(message instanceof FreespaceMsgInMotionEngineOutput)) { + new RobobuggyLogicNotification("IMU gave us message other than motion engine output!", RobobuggyMessageLevel.NOTE); + return; + } + + FreespaceMsgInMotionEngineOutput imuMotionEngineOutput = ((FreespaceMsgInMotionEngineOutput) message); + + // check whether we are in the correct mode + if (imuMotionEngineOutput.getFormatSelect() != IMU_FORMAT_MODE) { + new RobobuggyLogicNotification("IMU is in an unexpected mode!", RobobuggyMessageLevel.EXCEPTION); + return; + } + + // get the data array, and start our tracker + int[] imuDataArray = imuMotionEngineOutput.getMeData(); + int dataArrayOffset = 0; + + // FF0 measures acceleration in the x, y, z coordinates (outlined on the device) + // x positive is to the right + // y positive is forward + // z positive is up + // reported in units of 0.01g + if (imuMotionEngineOutput.getFf0()) { + + int xAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xAccel = xAccelInHundredths / 100.0; + dataArrayOffset += 2; + + int yAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yAccel = yAccelInHundredths / 100.0; + dataArrayOffset += 2; + + int zAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zAccel = zAccelInHundredths / 100.0; + dataArrayOffset += 2; + + accelerationPub.publish(new IMUAccelerationMessage(xAccel, yAccel, zAccel)); + + } + + // FF1 measures linear acceleration in the xyz plane (outlined on the device) + // x positive is right + // y positive is forward + // z positive is up + // reported in units of 0.01g + if (imuMotionEngineOutput.getFf1()) { + + int xAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xAccel = xAccelInHundredths / 100.0; + dataArrayOffset += 2; + + int yAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yAccel = yAccelInHundredths / 100.0; + dataArrayOffset += 2; + + int zAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zAccel = zAccelInHundredths / 100.0; + dataArrayOffset += 2; + + linearAccelerationPub.publish(new IMULinearAccelerationMessage(xAccel, yAccel, zAccel)); + + } + + // FF2 measures angular velocity + // +x is tilt up (pitch) + // +y is tilt right (roll) + // +z is turn left (yaw) + // reported in units of 0.1 degrees/sec + if (imuMotionEngineOutput.getFf2()) { + + int xAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xAngularVel = xAngularVelocityInTenths / 10.0; + dataArrayOffset += 2; + + int yAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yAngularVel = yAngularVelocityInTenths / 10.0; + dataArrayOffset += 2; + + int zAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zAngularVel = zAngularVelocityInTenths / 10.0; + dataArrayOffset += 2; + + angularVelocityPub.publish(new IMUAngularVelocityMessage(xAngularVel, yAngularVel, zAngularVel)); + + } + + // FF3 measures the magnetometer with respect to the device frame of reference + // +x is forward + // +y is to the right + // +z is down + // reported in units of 0.001 gauss + if (imuMotionEngineOutput.getFf3()) { + + int xMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xMag = xMagInThousandths / 1000.0; + dataArrayOffset += 2; + + int yMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yMag = yMagInThousandths / 1000.0; + dataArrayOffset += 2; + + int zMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zMag = zMagInThousandths / 1000.0; + dataArrayOffset += 2; + + magnetometerPub.publish(new MagneticMeasurement(xMag, yMag, zMag)); + + } + + // FF4 measures the inclination + // +x is tilt up (pitch) + // +y is tilt right (roll) + // +z is turn left (yaw) + // reported in units of 0.1 degrees + if (imuMotionEngineOutput.getFf4()) { + + int xInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double xInclination = xInclinationInTenths / 10.0; + dataArrayOffset += 2; + + int yInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double yInclination = yInclinationInTenths / 10.0; + dataArrayOffset += 2; + + int zInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double zInclination = zInclinationInTenths / 10.0; + dataArrayOffset += 2; + + inclinationPub.publish(new IMUInclinationMessage(xInclination, yInclination, zInclination)); + + } + + // FF5 measures the compass heading + // reported in units of 0.1 degrees + if (imuMotionEngineOutput.getFf5()) { + + int degreesInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); + double degrees = degreesInTenths/10.0; + dataArrayOffset += 2; + + compassHeadingPub.publish(new IMUCompassMessage(degrees)); + + } + + // FF6 measures angular position + // Gives 4 quaternions (dimensionless) that are expressed as Q14 decimals + if (imuMotionEngineOutput.getFf6()) { + + double w = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); + dataArrayOffset += 2; + double x = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); + dataArrayOffset += 2; + double y = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); + dataArrayOffset += 2; + double z = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); + dataArrayOffset += 2; + + double r11 = 1 - 2 * y * y - 2 * z * z; + double r12 = 2 * x * y - 2 * z * w; + double r13 = 2 * x * z + 2 * y * w; + double r21 = 2 * x * y + 2 * z * w; + double r22 = 1 - 2 * x * x - 2 * z * z; + double r23 = 2 * y * z - 2 * x * w; + double r31 = 2 * x * z - 2 * y * w; + double r32 = 2 * y * z + 2 * x * w; + double r33 = 1 - 2 * x * x - 2 * y * y; + + //TODO normalize + //extracts + double[][] rot = { + { r11, r12, r13 }, + { r21, r22, r23 }, + { r31, r32, r33 } + }; + + angularPositionPub.publish(new IMUAngularPositionMessage(rot)); + + } + + } + + /** + * Returns a 2-byte data value from the IMU's data array + * @param data the byte array for the IMU + * @param offsetInBytes the offset into the data array where the halfword starts from. Note that this value does + * NOT get increased, you will need to keep track of an offset externally + * @return the halfword extracted + */ + protected int extractHalfWordFromDataArray(int[] data, int offsetInBytes) { + + int lsb = data[offsetInBytes]; + int msb = data[offsetInBytes + 1]; + return ((msb << 8) | lsb); + + } + + + /** + * Converts from fixed point to floating point + * + * Some of the imu data is expressed as fixed point numbers, with the format of Q{N}, + * where Q represents 'fixed point', and N represents the point. Q10 represents a + * fixed point number with 1 sign bit, 5 integer bits and 10 fractional bits + * + * More info is found on Page 18 + * + * The algorithm is as follows on Wikipedia: + * https://en.wikipedia.org/wiki/Q_(number_format)#Q_to_float + * + * @param q the 16-bit fixed-point number + * @param n the location of the point + * @return Converted floating point val + */ + protected double convertQNToDouble(int q, int n) { + + // convert it to a double + double qDouble = (double) q; + + // get the sign bit + int signBit = (q >> 15) & 0x1; + int sign = 0; + if (signBit == 0) { + sign = 1; + } + else { + sign = -1; + } + + // multiply it by 2^(-n) + return sign * qDouble * Math.pow(2, -1 * n); + + } + + + @Override + public void handleSend(Device device, HidOutMsg hidOutMsg, int i) { + + } + + @Override + public void notifyRemoved(Device device) { + + } + + @Override + public void freespaceDeviceInserted(Device device) { + hillcrestImu = device; + + // open the device + FreespaceErrorCodes openResponse = hillcrestImu.open(this); + if (!openResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { + new RobobuggyLogicNotification("Error opening IMU!", RobobuggyMessageLevel.EXCEPTION); + return; + } + + int modeAndStatus = 8; // bit 3 set gives Full Motion On mode - page 41 + int packetSelect = 8; // 8 = MotionEngineOutput - page 41 + int formatSelect = IMU_FORMAT_MODE; // Mode 1 - page 21 + + FreespaceMsgOutDataModeControlV2Request configMsg = new FreespaceMsgOutDataModeControlV2Request(); + + configMsg.setModeAndStatus(modeAndStatus); + configMsg.setPacketSelect(packetSelect); + configMsg.setFormatSelect(formatSelect); + + // we want to make sure that we get all the packets + // the Format Flags are basically a way to enable each individual sensor - page 41 + configMsg.setFf0(true); + configMsg.setFf1(true); + configMsg.setFf2(true); + configMsg.setFf3(true); + configMsg.setFf4(true); + configMsg.setFf5(true); + configMsg.setFf6(true); + configMsg.setFf7(true); + + // send down the config info, and get the response + FreespaceErrorCodes configResponse = hillcrestImu.sendMessage(configMsg); + if (!configResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { + new RobobuggyLogicNotification("Error configuring IMU!", RobobuggyMessageLevel.EXCEPTION); + } + } + + @Override + public void freespaceDeviceRemoved(Device device) { + // close doesn't return a status + hillcrestImu.close(); + hillcrestImu = null; + } +} From cb30eb5758f17d8ea053ce8115de73b70900255d Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 29 Nov 2016 01:29:08 -0500 Subject: [PATCH 20/25] oops forgot how to use mv --- .../java_src/HillCrestImuNode.java | 381 ------------------ 1 file changed, 381 deletions(-) delete mode 100644 real_time/surface_src/java_src/HillCrestImuNode.java diff --git a/real_time/surface_src/java_src/HillCrestImuNode.java b/real_time/surface_src/java_src/HillCrestImuNode.java deleted file mode 100644 index c3dc18a8..00000000 --- a/real_time/surface_src/java_src/HillCrestImuNode.java +++ /dev/null @@ -1,381 +0,0 @@ -package com.roboclub.robobuggy.nodes.sensors; - -import com.hcrest.jfreespace.Device; -import com.hcrest.jfreespace.DeviceListenerInterface; -import com.hcrest.jfreespace.Discovery; -import com.hcrest.jfreespace.DiscoveryListenerInterface; -import com.hcrest.jfreespace.FreespaceErrorCodes; -import com.hcrest.jfreespace.inreport.FreespaceMsgInMotionEngineOutput; -import com.hcrest.jfreespace.inreport.HidInMsg; -import com.hcrest.jfreespace.outreport.FreespaceMsgOutDataModeControlV2Request; -import com.hcrest.jfreespace.outreport.HidOutMsg; -import com.roboclub.robobuggy.main.RobobuggyLogicNotification; -import com.roboclub.robobuggy.main.RobobuggyMessageLevel; -import com.roboclub.robobuggy.messages.IMUAccelerationMessage; -import com.roboclub.robobuggy.messages.IMUAngularPositionMessage; -import com.roboclub.robobuggy.messages.IMUAngularVelocityMessage; -import com.roboclub.robobuggy.messages.IMUCompassMessage; -import com.roboclub.robobuggy.messages.IMUInclinationMessage; -import com.roboclub.robobuggy.messages.IMULinearAccelerationMessage; -import com.roboclub.robobuggy.messages.MagneticMeasurement; -import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode; -import com.roboclub.robobuggy.nodes.baseNodes.BuggyDecoratorNode; -import com.roboclub.robobuggy.ros.NodeChannel; -import com.roboclub.robobuggy.ros.Publisher; - -/** - * A node to communicate with the Hillcrest Freespace 9DOF IMU - * - * Comments will make callbacks to page numbers on the datasheet/reference manual, which can be found here: - * http://hillcrestlabs.com/resources/download-materials/download-info/hcomm-reference-manual/ - */ -public class HillCrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { - - private Device hillcrestImu; - - // Constants - private static final int IMU_FORMAT_MODE = 1; // Mode 1 - page 21 - - // Publishers for the different messages - private Publisher accelerationPub; - private Publisher linearAccelerationPub; - private Publisher angularVelocityPub; - private Publisher magnetometerPub; - private Publisher inclinationPub; - private Publisher compassHeadingPub; - private Publisher angularPositionPub; - - /** - * Creates a new Hillcrest IMU node - */ - public HillCrestImuNode() { - super(new BuggyBaseNode(NodeChannel.IMU), "Hillcrest IMU"); - - // initialize the publishers - accelerationPub = new Publisher(NodeChannel.IMU_ACCELERATION.getMsgPath()); - linearAccelerationPub = new Publisher(NodeChannel.IMU_LINEAR_ACC.getMsgPath()); - angularVelocityPub = new Publisher(NodeChannel.IMU_ANG_VEL.getMsgPath()); - magnetometerPub = new Publisher(NodeChannel.IMU_MAGNETIC.getMsgPath()); - inclinationPub = new Publisher(NodeChannel.IMU_INCLINATION.getMsgPath()); - compassHeadingPub = new Publisher(NodeChannel.IMU_COMPASS.getMsgPath()); - angularPositionPub = new Publisher(NodeChannel.IMU_ANG_POS.getMsgPath()); - } - - @Override - protected boolean startDecoratorNode() { - // look for an IMU, and add this as a listener - Discovery.getInstance().addListener(this); - - return true; - } - - @Override - protected boolean shutdownDecoratorNode() { - Discovery.getInstance().removeListener(this); - return true; - } - - /** - * This method parses the IMU data array by using the spec on Pages 22 and 23 of the - * reference manual - * - * @param device the IMU in question - * @param message the message from the IMU - * @param i [Unknown] - * @param l [Unknown] - */ - @Override - public void handleRead(Device device, HidInMsg message, int i, long l) { - - // first check whether the message was the kind we're looking for - // at this point we are only handling MotionEngineOutput messages - if (!(message instanceof FreespaceMsgInMotionEngineOutput)) { - new RobobuggyLogicNotification("IMU gave us message other than motion engine output!", RobobuggyMessageLevel.NOTE); - return; - } - - FreespaceMsgInMotionEngineOutput imuMotionEngineOutput = ((FreespaceMsgInMotionEngineOutput) message); - - // check whether we are in the correct mode - if (imuMotionEngineOutput.getFormatSelect() != IMU_FORMAT_MODE) { - new RobobuggyLogicNotification("IMU is in an unexpected mode!", RobobuggyMessageLevel.EXCEPTION); - return; - } - - // get the data array, and start our tracker - int[] imuDataArray = imuMotionEngineOutput.getMeData(); - int dataArrayOffset = 0; - - // FF0 measures acceleration in the x, y, z coordinates (outlined on the device) - // x positive is to the right - // y positive is forward - // z positive is up - // reported in units of 0.01g - if (imuMotionEngineOutput.getFf0()) { - - int xAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double xAccel = xAccelInHundredths / 100.0; - dataArrayOffset += 2; - - int yAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double yAccel = yAccelInHundredths / 100.0; - dataArrayOffset += 2; - - int zAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double zAccel = zAccelInHundredths / 100.0; - dataArrayOffset += 2; - - accelerationPub.publish(new IMUAccelerationMessage(xAccel, yAccel, zAccel)); - - } - - // FF1 measures linear acceleration in the xyz plane (outlined on the device) - // x positive is right - // y positive is forward - // z positive is up - // reported in units of 0.01g - if (imuMotionEngineOutput.getFf1()) { - - int xAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double xAccel = xAccelInHundredths / 100.0; - dataArrayOffset += 2; - - int yAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double yAccel = yAccelInHundredths / 100.0; - dataArrayOffset += 2; - - int zAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double zAccel = zAccelInHundredths / 100.0; - dataArrayOffset += 2; - - linearAccelerationPub.publish(new IMULinearAccelerationMessage(xAccel, yAccel, zAccel)); - - } - - // FF2 measures angular velocity - // +x is tilt up (pitch) - // +y is tilt right (roll) - // +z is turn left (yaw) - // reported in units of 0.1 degrees/sec - if (imuMotionEngineOutput.getFf2()) { - - int xAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double xAngularVel = xAngularVelocityInTenths / 10.0; - dataArrayOffset += 2; - - int yAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double yAngularVel = yAngularVelocityInTenths / 10.0; - dataArrayOffset += 2; - - int zAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double zAngularVel = zAngularVelocityInTenths / 10.0; - dataArrayOffset += 2; - - angularVelocityPub.publish(new IMUAngularVelocityMessage(xAngularVel, yAngularVel, zAngularVel)); - - } - - // FF3 measures the magnetometer with respect to the device frame of reference - // +x is forward - // +y is to the right - // +z is down - // reported in units of 0.001 gauss - if (imuMotionEngineOutput.getFf3()) { - - int xMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double xMag = xMagInThousandths / 1000.0; - dataArrayOffset += 2; - - int yMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double yMag = yMagInThousandths / 1000.0; - dataArrayOffset += 2; - - int zMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double zMag = zMagInThousandths / 1000.0; - dataArrayOffset += 2; - - magnetometerPub.publish(new MagneticMeasurement(xMag, yMag, zMag)); - - } - - // FF4 measures the inclination - // +x is tilt up (pitch) - // +y is tilt right (roll) - // +z is turn left (yaw) - // reported in units of 0.1 degrees - if (imuMotionEngineOutput.getFf4()) { - - int xInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double xInclination = xInclinationInTenths / 10.0; - dataArrayOffset += 2; - - int yInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double yInclination = yInclinationInTenths / 10.0; - dataArrayOffset += 2; - - int zInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double zInclination = zInclinationInTenths / 10.0; - dataArrayOffset += 2; - - inclinationPub.publish(new IMUInclinationMessage(xInclination, yInclination, zInclination)); - - } - - // FF5 measures the compass heading - // reported in units of 0.1 degrees - if (imuMotionEngineOutput.getFf5()) { - - int degreesInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); - double degrees = degreesInTenths/10.0; - dataArrayOffset += 2; - - compassHeadingPub.publish(new IMUCompassMessage(degrees)); - - } - - // FF6 measures angular position - // Gives 4 quaternions (dimensionless) that are expressed as Q14 decimals - if (imuMotionEngineOutput.getFf6()) { - - double w = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); - dataArrayOffset += 2; - double x = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); - dataArrayOffset += 2; - double y = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); - dataArrayOffset += 2; - double z = convertQNToDouble(extractHalfWordFromDataArray(imuDataArray, dataArrayOffset), 14); - dataArrayOffset += 2; - - double r11 = 1 - 2 * y * y - 2 * z * z; - double r12 = 2 * x * y - 2 * z * w; - double r13 = 2 * x * z + 2 * y * w; - double r21 = 2 * x * y + 2 * z * w; - double r22 = 1 - 2 * x * x - 2 * z * z; - double r23 = 2 * y * z - 2 * x * w; - double r31 = 2 * x * z - 2 * y * w; - double r32 = 2 * y * z + 2 * x * w; - double r33 = 1 - 2 * x * x - 2 * y * y; - - //TODO normalize - //extracts - double[][] rot = { - { r11, r12, r13 }, - { r21, r22, r23 }, - { r31, r32, r33 } - }; - - angularPositionPub.publish(new IMUAngularPositionMessage(rot)); - - } - - } - - /** - * Returns a 2-byte data value from the IMU's data array - * @param data the byte array for the IMU - * @param offsetInBytes the offset into the data array where the halfword starts from. Note that this value does - * NOT get increased, you will need to keep track of an offset externally - * @return the halfword extracted - */ - protected int extractHalfWordFromDataArray(int[] data, int offsetInBytes) { - - int lsb = data[offsetInBytes]; - int msb = data[offsetInBytes + 1]; - return ((msb << 8) | lsb); - - } - - - /** - * Converts from fixed point to floating point - * - * Some of the imu data is expressed as fixed point numbers, with the format of Q{N}, - * where Q represents 'fixed point', and N represents the point. Q10 represents a - * fixed point number with 1 sign bit, 5 integer bits and 10 fractional bits - * - * More info is found on Page 18 - * - * The algorithm is as follows on Wikipedia: - * https://en.wikipedia.org/wiki/Q_(number_format)#Q_to_float - * - * @param q the 16-bit fixed-point number - * @param n the location of the point - * @return Converted floating point val - */ - protected double convertQNToDouble(int q, int n) { - - // convert it to a double - double qDouble = (double) q; - - // get the sign bit - int signBit = (q >> 15) & 0x1; - int sign = 0; - if (signBit == 0) { - sign = 1; - } - else { - sign = -1; - } - - // multiply it by 2^(-n) - return sign * qDouble * Math.pow(2, -1 * n); - - } - - - @Override - public void handleSend(Device device, HidOutMsg hidOutMsg, int i) { - - } - - @Override - public void notifyRemoved(Device device) { - - } - - @Override - public void freespaceDeviceInserted(Device device) { - hillcrestImu = device; - - // open the device - FreespaceErrorCodes openResponse = hillcrestImu.open(this); - if (!openResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { - new RobobuggyLogicNotification("Error opening IMU!", RobobuggyMessageLevel.EXCEPTION); - return; - } - - int modeAndStatus = 8; // bit 3 set gives Full Motion On mode - page 41 - int packetSelect = 8; // 8 = MotionEngineOutput - page 41 - int formatSelect = IMU_FORMAT_MODE; // Mode 1 - page 21 - - FreespaceMsgOutDataModeControlV2Request configMsg = new FreespaceMsgOutDataModeControlV2Request(); - - configMsg.setModeAndStatus(modeAndStatus); - configMsg.setPacketSelect(packetSelect); - configMsg.setFormatSelect(formatSelect); - - // we want to make sure that we get all the packets - // the Format Flags are basically a way to enable each individual sensor - page 41 - configMsg.setFf0(true); - configMsg.setFf1(true); - configMsg.setFf2(true); - configMsg.setFf3(true); - configMsg.setFf4(true); - configMsg.setFf5(true); - configMsg.setFf6(true); - configMsg.setFf7(true); - - // send down the config info, and get the response - FreespaceErrorCodes configResponse = hillcrestImu.sendMessage(configMsg); - if (!configResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { - new RobobuggyLogicNotification("Error configuring IMU!", RobobuggyMessageLevel.EXCEPTION); - } - } - - @Override - public void freespaceDeviceRemoved(Device device) { - // close doesn't return a status - hillcrestImu.close(); - hillcrestImu = null; - } -} From bea9ced7d6778ff9e9a94a928fb6438fb4e0ee6e Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Tue, 29 Nov 2016 01:31:31 -0500 Subject: [PATCH 21/25] ok I think this will fix it for sure --- .../{HillcrestImuNodeTest.java => HillCrestImuNodeTest.java} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/{HillcrestImuNodeTest.java => HillCrestImuNodeTest.java} (100%) diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNodeTest.java similarity index 100% rename from real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillcrestImuNodeTest.java rename to real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNodeTest.java From 0cb7f3e1c9981d9c36958258d2fc8d2109d7953a Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Fri, 2 Dec 2016 21:14:43 -0500 Subject: [PATCH 22/25] clarified comments --- .../robobuggy/nodes/sensors/HillCrestImuNodeTest.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNodeTest.java b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNodeTest.java index 7e51a998..74c2c781 100644 --- a/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNodeTest.java +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNodeTest.java @@ -14,7 +14,8 @@ public class HillCrestImuNodeTest { private HillCrestImuNode testNode; /** - * setup done before each test + * Before each test case, this method is run + * This method basically just resets the test node */ @Before public void setUp() { @@ -22,7 +23,8 @@ public void setUp() { } /** - * work that's done after each test + * After each test case, this method runs + * Right now this method doesn't do anything specifically */ @After public void tearDown() { From db8b0e71b62b7fc0c9e7435e3f59bc4d2c787fe0 Mon Sep 17 00:00:00 2001 From: VivaanBahl Date: Sat, 3 Dec 2016 02:01:54 -0500 Subject: [PATCH 23/25] Update .gitignore --- .gitignore | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.gitignore b/.gitignore index b2e14832..9f4d730d 100644 --- a/.gitignore +++ b/.gitignore @@ -38,9 +38,5 @@ real_time/surface_src/java_src/logs real_time/surface_src/opencv/ cwthbntv.nci.txt *.iml -real_time/surface_src/java_src/Alice/test.mp4 -t30euete.f4z.txt -bytes from arduino.txt -other.txt real_time/surface_src/java_src/Alice/.idea real_time/surface_src/java_src/Alice/library/protobuf-3.0.0 From a98c93395d89e7bf1e73a37c01c3a2aa8f20b2b5 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 3 Dec 2016 02:03:01 -0500 Subject: [PATCH 24/25] addressed pull request comments - fixed error string - removed unused class --- .../messages/HillCrestIMUMessage.java | 33 ------------------- .../nodes/sensors/HillCrestImuNode.java | 2 +- 2 files changed, 1 insertion(+), 34 deletions(-) delete mode 100644 real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java deleted file mode 100644 index 56506515..00000000 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java +++ /dev/null @@ -1,33 +0,0 @@ -package com.roboclub.robobuggy.messages; - -/** - * A temp message for storing the hilcrest imu's raw data - * - * @author Trevor Decker - */ -@Deprecated -public class HillCrestIMUMessage extends BaseMessage { - private String message = ""; - - public static final String VERSION_ID = "hillcrest_imuV0.0"; - - /** - * constructor for the datatype - * - * @param message message to encode - */ - public HillCrestIMUMessage(String message) { - this.message = message; - // TODO Auto-generated constructor stub - } - - /** - * evaluates to a string encoding of the message - * - * @return string encoding of the message - */ - public String getMessage() { - return message; - } - -} diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java index c3dc18a8..d45e6418 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java @@ -90,7 +90,7 @@ public void handleRead(Device device, HidInMsg message, int i, long l) { // first check whether the message was the kind we're looking for // at this point we are only handling MotionEngineOutput messages if (!(message instanceof FreespaceMsgInMotionEngineOutput)) { - new RobobuggyLogicNotification("IMU gave us message other than motion engine output!", RobobuggyMessageLevel.NOTE); + new RobobuggyLogicNotification("IMU gave us a message other than motion engine output!", RobobuggyMessageLevel.NOTE); return; } From 26a45bd4eed90e53980d0d6f8efb0851876b8621 Mon Sep 17 00:00:00 2001 From: Vivaan Bahl Date: Sat, 3 Dec 2016 02:20:24 -0500 Subject: [PATCH 25/25] addressed PR comments - clarified unknown arguments --- .../com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java index d45e6418..504bf017 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java @@ -79,6 +79,9 @@ protected boolean shutdownDecoratorNode() { * This method parses the IMU data array by using the spec on Pages 22 and 23 of the * reference manual * + * Note that parameters i and l are unknown since this overrides a method given to us in a + * library without documentation. We do not use these parameters, and so ignore them. + * * @param device the IMU in question * @param message the message from the IMU * @param i [Unknown]