diff --git a/.gitignore b/.gitignore index 2406451b..9f4d730d 100644 --- a/.gitignore +++ b/.gitignore @@ -38,5 +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 +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/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 0aa9fe22..00000000 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/messages/HillCrestIMUMessage.java +++ /dev/null @@ -1,32 +0,0 @@ -package com.roboclub.robobuggy.messages; - -/** - * A temp message for storing the hilcrest imu's raw data - * - * @author Trevor Decker - */ -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/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/nodes/sensors/HillCrestImuNode.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNode.java index 427cb84e..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 @@ -4,165 +4,250 @@ 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.IMUCompassMessage; 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; /** - * Driver for the Hillcrest IMUs + * A node to communicate with the Hillcrest Freespace 9DOF IMU * - * @author Trevor Decker - * @author Sean Buckley + * 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 implements DiscoveryListenerInterface, DeviceListenerInterface, com.roboclub.robobuggy.ros.Node { - private Device thisDevice; +public class HillCrestImuNode extends BuggyDecoratorNode implements DeviceListenerInterface, DiscoveryListenerInterface { + + private Device hillcrestImu; - 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()); + // 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; /** - * Constructor for the hillcrest imu + * Creates a new Hillcrest IMU node */ public HillCrestImuNode() { - super(); - Discovery discover = Discovery.getInstance(); - discover.addListener(this); + 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 + * + * 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] + * @param l [Unknown] + */ @Override - public void handleRead(Device arg0, HidInMsg arg1, int arg2, long arg3) { - //TODO handle other kinds of messages - if (!(arg1 instanceof FreespaceMsgInMotionEngineOutput)) { + 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 a message other than motion engine output!", RobobuggyMessageLevel.NOTE); return; } - FreespaceMsgInMotionEngineOutput m = (FreespaceMsgInMotionEngineOutput) arg1; + FreespaceMsgInMotionEngineOutput imuMotionEngineOutput = ((FreespaceMsgInMotionEngineOutput) message); - // we can only parse format 1 for now - if (m.getFormatSelect() != 1) { + // 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; } - int[] data = m.getMeData(); - int offsetInBytes = 0; + // get the data array, and start our tracker + int[] imuDataArray = imuMotionEngineOutput.getMeData(); + int dataArrayOffset = 0; - // FF0 is acceleration for Format 1 + // 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 (m.getFf0()) { - int xAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); + if (imuMotionEngineOutput.getFf0()) { + + int xAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double xAccel = xAccelInHundredths / 100.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int yAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); + int yAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double yAccel = yAccelInHundredths / 100.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int zAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); + int zAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double zAccel = zAccelInHundredths / 100.0; - offsetInBytes += 2; + dataArrayOffset += 2; - accelPub.publish(new IMUAccelerationMessage(xAccel, yAccel, zAccel)); + accelerationPub.publish(new IMUAccelerationMessage(xAccel, yAccel, zAccel)); } - //ff1 is linear acceleration for Format 1 + + // 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 (m.getFf1()) { - int xAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); + if (imuMotionEngineOutput.getFf1()) { + + int xAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double xAccel = xAccelInHundredths / 100.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int yAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); + int yAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double yAccel = yAccelInHundredths / 100.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int zAccelInHundredths = extractHalfWordFromDataArray(data, offsetInBytes); + int zAccelInHundredths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double zAccel = zAccelInHundredths / 100.0; - offsetInBytes += 2; + dataArrayOffset += 2; + + linearAccelerationPub.publish(new IMULinearAccelerationMessage(xAccel, yAccel, zAccel)); - 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); + + // 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; - offsetInBytes += 2; + dataArrayOffset += 2; - int yAngularVelocityInTenths = extractHalfWordFromDataArray(data, offsetInBytes); + int yAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double yAngularVel = yAngularVelocityInTenths / 10.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int zAngularVelocityInTenths = extractHalfWordFromDataArray(data, offsetInBytes); + int zAngularVelocityInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double zAngularVel = zAngularVelocityInTenths / 10.0; - offsetInBytes += 2; + dataArrayOffset += 2; + + angularVelocityPub.publish(new IMUAngularVelocityMessage(xAngularVel, yAngularVel, zAngularVel)); - angVelPub.publish(new IMUAngularVelocityMessage(xAngularVel, yAngularVel, zAngularVel)); } - //ff3 is magnetometer for Format 1 + + // 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 (m.getFf3()) { - int xMagInThousandths = extractHalfWordFromDataArray(data, offsetInBytes); + if (imuMotionEngineOutput.getFf3()) { + + int xMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double xMag = xMagInThousandths / 1000.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int yMagInThousandths = extractHalfWordFromDataArray(data, offsetInBytes); + int yMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double yMag = yMagInThousandths / 1000.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int zMagInThousandths = extractHalfWordFromDataArray(data, offsetInBytes); + int zMagInThousandths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double zMag = zMagInThousandths / 1000.0; - offsetInBytes += 2; + dataArrayOffset += 2; + + magnetometerPub.publish(new MagneticMeasurement(xMag, yMag, zMag)); - magPub.publish(new MagneticMeasurement(xMag, yMag, zMag)); } - //ff4 is inclination + + // 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 (m.getFf4()) { - int xInclinationInTenths = extractHalfWordFromDataArray(data, offsetInBytes); + if (imuMotionEngineOutput.getFf4()) { + + int xInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double xInclination = xInclinationInTenths / 10.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int yInclinationInTenths = extractHalfWordFromDataArray(data, offsetInBytes); + int yInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double yInclination = yInclinationInTenths / 10.0; - offsetInBytes += 2; + dataArrayOffset += 2; - int zInclinationInTenths = extractHalfWordFromDataArray(data, offsetInBytes); + int zInclinationInTenths = extractHalfWordFromDataArray(imuDataArray, dataArrayOffset); double zInclination = zInclinationInTenths / 10.0; - offsetInBytes += 2; + dataArrayOffset += 2; inclinationPub.publish(new IMUInclinationMessage(xInclination, yInclination, zInclination)); + } - //ff5 is compass for format 1 - if (m.getFf5()) { - int degreesInTenths = extractHalfWordFromDataArray(data, offsetInBytes); + + // 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; - offsetInBytes += 2; + dataArrayOffset += 2; + + compassHeadingPub.publish(new IMUCompassMessage(degrees)); - 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); + + // 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; @@ -182,114 +267,118 @@ public void handleRead(Device arg0, HidInMsg arg1, int arg2, long arg3) { { r31, r32, r33 } }; - angPosPub.publish(new IMUAngularPositionMessage(rot)); - offsetInBytes += 8; + angularPositionPub.publish(new IMUAngularPositionMessage(rot)); + } } - @Override - public void handleSend(Device arg0, HidOutMsg arg1, int arg2) { - // TODO Auto-generated method stub + /** + * 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); } - @Override - public void notifyRemoved(Device arg0) { - // TODO Auto-generated method stub - } + /** + * 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) { - @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); - } + // convert it to a double + double qDouble = (double) q; - @Override - public void freespaceDeviceRemoved(Device arg0) { - // TODO Auto-generated method stub - thisDevice = null; + // 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 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 + public void handleSend(Device device, HidOutMsg hidOutMsg, int i) { } @Override - public String getName() { - // TODO Auto-generated method stub - return "hillcrestImu"; - } + public void notifyRemoved(Device device) { - /** - * 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); + @Override + public void freespaceDeviceInserted(Device device) { + hillcrestImu = device; - if (sT >= 1 << 15) { - sT = sT - (1 << 17 - 1); + // open the device + FreespaceErrorCodes openResponse = hillcrestImu.open(this); + if (!openResponse.equals(FreespaceErrorCodes.FREESPACE_SUCCESS)) { + new RobobuggyLogicNotification("Error opening IMU!", RobobuggyMessageLevel.EXCEPTION); + return; } - double d = sT.doubleValue(); - return Math.pow(2, -1 * qn) * d; - + 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); + } } - /** - * 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); + @Override + public void freespaceDeviceRemoved(Device device) { + // close doesn't return a status + hillcrestImu.close(); + hillcrestImu = null; } - } 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 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 new file mode 100644 index 00000000..74c2c781 --- /dev/null +++ b/real_time/surface_src/java_src/Alice/src/test/java/com/roboclub/robobuggy/nodes/sensors/HillCrestImuNodeTest.java @@ -0,0 +1,102 @@ +package com.roboclub.robobuggy.nodes.sensors; + +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 HillCrestImuNodeTest { + + private HillCrestImuNode testNode; + + /** + * Before each test case, this method is run + * This method basically just resets the test node + */ + @Before + public void setUp() { + testNode = new HillCrestImuNode(); + } + + /** + * After each test case, this method runs + * Right now this method doesn't do anything specifically + */ + @After + public void tearDown() { + } + + /** + * Tests that we can safely create an instance of the node + * + * No input arguments + * Expects no output or errors + */ + @Test + public void testCreation() { + HillCrestImuNode imu = new HillCrestImuNode(); + 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.1417; + 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