From f3593c6986c52dcf63e68f8dfa4cdf493b342447 Mon Sep 17 00:00:00 2001 From: thomasclark Date: Wed, 25 Jun 2014 16:25:39 -0400 Subject: [PATCH] CANJaguar in Java now requires control mode configuration data at compile time Change-Id: I80b63e876d330d693375d69137c78e487c03c1c7 --- wpilibc/wpilibC++/include/CANJaguar.h | 1 - .../java/edu/wpi/first/wpilibj/CANJaguar.java | 449 +++++++++++++----- 2 files changed, 326 insertions(+), 124 deletions(-) diff --git a/wpilibc/wpilibC++/include/CANJaguar.h b/wpilibc/wpilibC++/include/CANJaguar.h index 87273f65a0..f6db4a47ac 100644 --- a/wpilibc/wpilibC++/include/CANJaguar.h +++ b/wpilibc/wpilibC++/include/CANJaguar.h @@ -33,7 +33,6 @@ public: static constexpr double kApproxBusVoltage = 12.0; // Control mode tags - static const struct NoneStruct {} None; static const struct EncoderStruct {} Encoder; static const struct QuadEncoderStruct {} QuadEncoder; static const struct PotentiometerStruct {} Potentiometer; diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java index 65850f0c12..ec8840b7e6 100644 --- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java +++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java @@ -41,10 +41,19 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW private static final int kFullMessageIDMask = CANJNI.CAN_MSGID_API_M | CANJNI.CAN_MSGID_MFR_M | CANJNI.CAN_MSGID_DTYPE_M; private static final int kSendMessagePeriod = 20; + // Control Mode tags + private static class EncoderTag {}; + public final static EncoderTag kEncoder = new EncoderTag(); + private static class QuadEncoderTag {}; + public final static QuadEncoderTag kQuadEncoder = new QuadEncoderTag(); + private static class PotentiometerTag {}; + public final static PotentiometerTag kPotentiometer = new PotentiometerTag(); + + /** - * Mode determines how the Jaguar is controlled + * Mode determines how the Jaguar is controlled, used internally */ - public enum ControlMode { + private enum ControlMode { PercentVbus, Current, Speed, Position, Voltage; } @@ -62,52 +71,6 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW public static final int kForwardLimit = 1; public static final int kReverseLimit = 2; - /** - * Determines which sensor to use for position reference. - */ - public enum PositionReference { - QuadEncoder((byte)0), Potentiometer((byte)1), None((byte)0xff); - - public byte value; - - public static PositionReference valueOf(byte value) { - for(PositionReference posRef : values()) { - if(posRef.value == value) { - return posRef; - } - } - - return null; - } - - private PositionReference(byte value) { - this.value = value; - } - } - - /** - * Determines which sensor to use for speed reference. - */ - public enum SpeedReference { - Encoder((byte)0), InvEncoder((byte)2), QuadEncoder((byte)3), None((byte)0xff); - - public byte value; - - public static SpeedReference valueOf(byte value) { - for(SpeedReference speedRef : values()) { - if(speedRef.value == value) { - return speedRef; - } - } - - return null; - } - - private SpeedReference(byte value) { - this.value = value; - } - } - /** * Determines how the Jaguar behaves when sending a zero signal. */ @@ -257,7 +220,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the output set-point value. + * set the output set-point value. * * The scale and the units depend on the mode the Jaguar is in. * In PercentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM Jaguar). @@ -267,7 +230,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW * In Position Mode, the outputValue is in Rotations. * * @param outputValue The set-point to sent to the motor controller. - * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup(). If 0, update immediately. + * @param syncGroup The update group to add this set() to, pending UpdateSyncGroup(). If 0, update immediately. */ public void set(double outputValue, byte syncGroup) { int messageID; @@ -340,7 +303,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW data[0] = 1; sendMessage(CANJNI.LM_API_STATUS_POWER, data, 1); - // Set all configuration data again. This will resend it to + // set all configuration data again. This will resend it to // Jaguar and mark everything as unverified. enableControl(); setSpeedReference(m_speedReference); @@ -406,7 +369,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_SPD_REF, CANJNI.CAN_MSGID_FULL_M, data); - SpeedReference speedRef = SpeedReference.valueOf(data[0]); + int speedRef = data[0]; if(m_speedReference == speedRef) { m_speedRefVerified = true; @@ -424,7 +387,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW try { getMessage(CANJNI.LM_API_POS_REF, CANJNI.CAN_MSGID_FULL_M, data); - PositionReference posRef = PositionReference.valueOf(data[0]); + int posRef = data[0]; if(m_positionReference == posRef) { m_posRefVerified = true; @@ -735,14 +698,14 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the reference source device for speed controller mode. + * set the reference source device for speed controller mode. * * Choose encoder as the source of speed feedback when in speed control mode. * - * @param reference Specify a SpeedReference. + * @param reference Specify a speed reference. */ - public void setSpeedReference(SpeedReference reference) { - sendMessage(CANJNI.LM_API_SPD_REF, new byte[] { reference.value }, 1); + private void setSpeedReference(int reference) { + sendMessage(CANJNI.LM_API_SPD_REF, new byte[] { (byte)reference }, 1); m_speedReference = reference; m_speedRefVerified = false; @@ -751,23 +714,23 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW /** * Get the reference source device for speed controller mode. * - * @return A SpeedReference indicating the currently selected reference + * @return A speed reference indicating the currently selected reference * device for speed controller mode. */ - public SpeedReference getSpeedReference() { + private int getSpeedReference() { return m_speedReference; } /** - * Set the reference source device for position controller mode. + * set the reference source device for position controller mode. * * Choose between using and encoder and using a potentiometer * as the source of position feedback when in position control mode. * - * @param reference Specify a PositionReference. + * @param reference Specify a position reference. */ - public void setPositionReference(PositionReference reference) { - sendMessage(CANJNI.LM_API_POS_REF, new byte[] { reference.value }, 1); + private void setPositionReference(int reference) { + sendMessage(CANJNI.LM_API_POS_REF, new byte[] { (byte)reference }, 1); m_positionReference = reference; m_posRefVerified = false; @@ -779,12 +742,12 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW * @return A PositionReference indicating the currently selected reference * device for position controller mode. */ - public PositionReference getPositionReference() { + private int getPositionReference() { return m_positionReference; } /** - * Set the P constant for the closed loop modes. + * set the P constant for the closed loop modes. * * @param p The proportional gain of the Jaguar's PID controller. */ @@ -814,7 +777,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the I constant for the closed loop modes. + * set the I constant for the closed loop modes. * * @param i The integral gain of the Jaguar's PID controller. */ @@ -844,7 +807,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the D constant for the closed loop modes. + * set the D constant for the closed loop modes. * * @param d The derivative gain of the Jaguar's PID controller. */ @@ -874,7 +837,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the P, I, and D constants for the closed loop modes. + * set the P, I, and D constants for the closed loop modes. * * @param p The proportional gain of the Jaguar's PID controller. * @param i The integral gain of the Jaguar's PID controller. @@ -987,6 +950,246 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } } + /** + * Enable controlling the motor voltage as a percentage of the bus voltage + * without any position or speed feedback. + */ + public void setPercentMode() + { + changeControlMode(ControlMode.PercentVbus); + setPositionReference(CANJNI.LM_REF_NONE); + setSpeedReference(CANJNI.LM_REF_NONE); + } + + /** + * Enable controlling the motor voltage as a percentage of the bus voltage, + * and enable speed sensing from a non-quadrature encoder. + * + * @param tag The constant CANJaguar.Encoder + * @param codesPerRev The counts per revolution on the encoder + */ + public void setPercentMode(EncoderTag tag, int codesPerRev) + { + changeControlMode(ControlMode.PercentVbus); + setPositionReference(CANJNI.LM_REF_NONE); + setSpeedReference(CANJNI.LM_REF_ENCODER); + configEncoderCodesPerRev(codesPerRev); + } + + /** + * Enable controlling the motor voltage as a percentage of the bus voltage, + * and enable position and speed sensing from a quadrature encoder + * + * @param tag The constant CANJaguar.QuadEncoder + * @param codesPerRev The counts per revolution on the encoder + */ + public void setPercentMode(QuadEncoderTag tag, int codesPerRev) + { + changeControlMode(ControlMode.PercentVbus); + setPositionReference(CANJNI.LM_REF_QUAD_ENCODER); + setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); + configEncoderCodesPerRev(codesPerRev); + } + + /** + * Enable controlling the motor voltage as a percentage of the bus voltage, + * and enable position sensing from a potentiometer and no speed feedback. + * + * @param tag The constant CANJaguar.Potentiometer + */ + public void setPercentMode(PotentiometerTag tag) + { + changeControlMode(ControlMode.PercentVbus); + setPositionReference(CANJNI.LM_REF_POT); + setSpeedReference(CANJNI.LM_REF_NONE); + configPotentiometerTurns(1); + } + + /** + * Enable controlling the motor current with a PID loop. + * + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ + public void setCurrentMode(double p, double i, double d) + { + changeControlMode(ControlMode.Current); + setPositionReference(CANJNI.LM_REF_NONE); + setSpeedReference(CANJNI.LM_REF_NONE); + setPID(p, i, d); + } + + /** + * Enable controlling the motor current with a PID loop, and enable speed + * sensing from a non-quadrature encoder. + * + * @param tag The constant CANJaguar.Encoder + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ + public void setCurrentMode(EncoderTag tag, int codesPerRev, double p, double i, double d) + { + changeControlMode(ControlMode.Current); + setPositionReference(CANJNI.LM_REF_NONE); + setSpeedReference(CANJNI.LM_REF_NONE); + configEncoderCodesPerRev(codesPerRev); + setPID(p, i, d); + } + + /** + * Enable controlling the motor current with a PID loop, and enable speed and + * position sensing from a quadrature encoder. + * + * @param endoer The constant CANJaguar.QuadEncoder + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ + public void setCurrentMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) + { + changeControlMode(ControlMode.Current); + setPositionReference(CANJNI.LM_REF_QUAD_ENCODER); + setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); + configEncoderCodesPerRev(codesPerRev); + setPID(p, i, d); + } + + /** + * Enable controlling the motor current with a PID loop, and enable position + * sensing from a potentiometer. + * + * @param tag The constant CANJaguar.Potentiometer + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ + public void setCurrentMode(PotentiometerTag tag, double p, double i, double d) + { + changeControlMode(ControlMode.Current); + setPositionReference(CANJNI.LM_REF_POT); + setSpeedReference(CANJNI.LM_REF_NONE); + configPotentiometerTurns(1); + setPID(p, i, d); + } + + /** + * Enable controlling the speed with a feedback loop from a non-quadrature + * encoder. + * + * @param tag The constant CANJaguar.Encoder + * @param codesPerRev The counts per revolution on the encoder + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ + public void setSpeedMode(EncoderTag tag, int codesPerRev, double p, double i, double d) + { + changeControlMode(ControlMode.Speed); + setPositionReference(CANJNI.LM_REF_NONE); + setSpeedReference(CANJNI.LM_REF_ENCODER); + configEncoderCodesPerRev(codesPerRev); + setPID(p, i, d); + } + + /** + * Enable controlling the speed with a feedback loop from a quadrature encoder. + * + * @param tag The constant CANJaguar.QuadEncoder + * @param codesPerRev The counts per revolution on the encoder + * @param p The proportional gain of the Jaguar's PID controller. + * @param i The integral gain of the Jaguar's PID controller. + * @param d The differential gain of the Jaguar's PID controller. + */ + public void setSpeedMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) + { + changeControlMode(ControlMode.Speed); + setPositionReference(CANJNI.LM_REF_QUAD_ENCODER); + setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); + configEncoderCodesPerRev(codesPerRev); + setPID(p, i, d); + } + + /** + * Enable controlling the position with a feedback loop using an encoder + * + * @param tag The constant CANJaguar.QuadEncoder + * + */ + public void setPositionMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) + { + changeControlMode(ControlMode.Position); + setPositionReference(CANJNI.LM_REF_QUAD_ENCODER); + configEncoderCodesPerRev(codesPerRev); + setPID(p, i, d); + } + + /** + * Enable controlling the position with a feedback loop using a potentiometer + */ + public void setPositionMode(PotentiometerTag tag, double p, double i, double d) + { + changeControlMode(ControlMode.Position); + setPositionReference(CANJNI.LM_REF_POT); + configPotentiometerTurns(1); + setPID(p, i, d); + } + + /** + * Enable controlling the motor voltage without any position or speed feedback. + */ + public void setVoltageMode() + { + changeControlMode(ControlMode.Voltage); + setPositionReference(CANJNI.LM_REF_NONE); + setSpeedReference(CANJNI.LM_REF_NONE); + } + + /** + * Enable controlling the motor voltage with speed feedback from a + * non-quadrature encoder and no position feedback. + * + * @param tag The constant CANJaguar.Encoder + * @param codesPerRev The counts per revolution on the encoder + */ + public void setVoltageMode(EncoderTag tag, int codesPerRev) + { + changeControlMode(ControlMode.Voltage); + setPositionReference(CANJNI.LM_REF_NONE); + setSpeedReference(CANJNI.LM_REF_ENCODER); + configEncoderCodesPerRev(codesPerRev); + } + + /** + * Enable controlling the motor voltage with position and speed feedback from a + * quadrature encoder + * + * @param tag The constant CANJaguar.QuadEncoder + * @param codesPerRev The counts per revolution on the encoder + */ + public void setVoltageMode(QuadEncoderTag tag, int codesPerRev) + { + changeControlMode(ControlMode.Voltage); + setPositionReference(CANJNI.LM_REF_QUAD_ENCODER); + setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); + configEncoderCodesPerRev(codesPerRev); + } + + /** + * Enable controlling the motor voltage with position feedback from a + * potentiometer and no speed feedback. + * + * @param tag The constant CANJaguar.Potentiometer + */ + public void setVoltageMode(PotentiometerTag tag) + { + changeControlMode(ControlMode.Voltage); + setPositionReference(CANJNI.LM_REF_POT); + setSpeedReference(CANJNI.LM_REF_NONE); + configPotentiometerTurns(1); + } + /** * Change the control mode of this Jaguar object. * @@ -995,7 +1198,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW * * @param controlMode The new mode. */ - public void changeControlMode(ControlMode controlMode) { + private void changeControlMode(ControlMode controlMode) { // Disable the previous mode disableControl(); @@ -1010,7 +1213,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW * * @return ControlMode that the Jag is in. */ - public ControlMode getControlMode() { + private ControlMode getControlMode() { return m_controlMode; } @@ -1146,7 +1349,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW * Get the status of any faults the Jaguar has detected. * * @return A bit-mask of faults defined by the "Faults" constants. - * @see kCurrentFault + * @see ControlMode.CurrentFault * @see kBusVoltageFault * @see kTemperatureFault * @see kGateDriverFault @@ -1163,10 +1366,10 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the maximum voltage change rate. + * set the maximum voltage change rate. * * When in PercentVbus or Voltage output mode, the rate at which the voltage changes can - * be limited to reduce current spikes. Set this to 0.0 to disable rate limiting. + * be limited to reduce current spikes. set this to 0.0 to disable rate limiting. * * @param rampRate The maximum rate of voltage change in Percent Voltage mode in V/s. */ @@ -1283,7 +1486,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the limit mode for position control mode. + * set the limit mode for position control mode. * * Use ConfigSoftPositionLimits or DisableSoftPositionLimits to set this * automatically. @@ -1293,7 +1496,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the position that if exceeded will disable the forward direction. + * set the position that if exceeded will disable the forward direction. * * Use ConfigSoftPositionLimits to set this and the limit mode automatically. */ @@ -1309,7 +1512,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Set the position that if exceeded will disable the reverse direction. + * set the position that if exceeded will disable the reverse direction. * * Use ConfigSoftPositionLimits to set this and the limit mode automatically. */ @@ -1365,8 +1568,8 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW // Parameters/configuration ControlMode m_controlMode; - SpeedReference m_speedReference = SpeedReference.None; - PositionReference m_positionReference = PositionReference.None; + int m_speedReference = CANJNI.LM_REF_NONE; + int m_positionReference = CANJNI.LM_REF_NONE; double m_p = 0.0; double m_i = 0.0; double m_d = 0.0; @@ -1466,61 +1669,61 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Send a message to the Jaguar. - * - * @param messageID The messageID to be used on the CAN bus (device number - * is added internally) - * @param data The up to 8 bytes of data to be sent with the message - * @param dataSize Specify how much of the data in "data" to send - * @param periodic If positive, tell Network Communications to send the - * message every "period" milliseconds. - */ + * Send a message to the Jaguar. + * + * @param messageID The messageID to be used on the CAN bus (device number + * is added internally) + * @param data The up to 8 bytes of data to be sent with the message + * @param dataSize Specify how much of the data in "data" to send + * @param periodic If positive, tell Network Communications to send the + * message every "period" milliseconds. + */ protected void sendMessage(int messageID, byte[] data, int dataSize, int period) { sendMessageHelper(messageID | m_deviceNumber, data, dataSize, period); } /** - * Send a message to the Jaguar, non-periodly - * - * @param messageID The messageID to be used on the CAN bus (device number - * is added internally) - * @param data The up to 8 bytes of data to be sent with the message - * @param dataSize Specify how much of the data in "data" to send - */ + * Send a message to the Jaguar, non-periodly + * + * @param messageID The messageID to be used on the CAN bus (device number + * is added internally) + * @param data The up to 8 bytes of data to be sent with the message + * @param dataSize Specify how much of the data in "data" to send + */ protected void sendMessage(int messageID, byte[] data, int dataSize) { sendMessage(messageID, data, dataSize, CANJNI.CAN_SEND_PERIOD_NO_REPEAT); } /** - * Request a message from the Jaguar, but don't wait for it to arrive. - * - * @param messageID The message to request - * @param periodic If positive, tell Network Communications to request the - * message every "period" milliseconds. - */ + * Request a message from the Jaguar, but don't wait for it to arrive. + * + * @param messageID The message to request + * @param periodic If positive, tell Network Communications to request the + * message every "period" milliseconds. + */ protected void requestMessage(int messageID, int period) { sendMessageHelper(messageID | m_deviceNumber, null, 0, period); } /** - * Request a message from the Jaguar, but don't wait for it to arrive. - * - * @param messageID The message to request - */ + * Request a message from the Jaguar, but don't wait for it to arrive. + * + * @param messageID The message to request + */ protected void requestMessage(int messageID) { requestMessage(messageID, CANJNI.CAN_SEND_PERIOD_NO_REPEAT); } /** - * Get a previously requested message. - * - * Jaguar always generates a message with the same message ID when replying. - * - * @param messageID The messageID to read from the CAN bus (device number is added internally) - * @param data The up to 8 bytes of data that was received with the message - * - * @throw CANMessageNotFoundException if there's not new message available - */ + * Get a previously requested message. + * + * Jaguar always generates a message with the same message ID when replying. + * + * @param messageID The messageID to read from the CAN bus (device number is added internally) + * @param data The up to 8 bytes of data that was received with the message + * + * @throw CANMessageNotFoundException if there's not new message available + */ protected void getMessage(int messageID, int messageMask, byte[] data) throws CANMessageNotFoundException { messageID |= m_deviceNumber; messageID &= CANJNI.CAN_MSGID_FULL_M; @@ -1611,21 +1814,21 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW } /** - * Unpack 16-bit data from a buffer in little-endian byte order - * @param buffer The buffer to unpack from - * @param offset The offset into he buffer to unpack - * @return The data that was unpacked - */ + * Unpack 16-bit data from a buffer in little-endian byte order + * @param buffer The buffer to unpack from + * @param offset The offset into he buffer to unpack + * @return The data that was unpacked + */ private static final short unpack16(byte[] buffer, int offset) { return (short) (((int) buffer[offset] & 0xFF) | (short) ((buffer[offset + 1] << 8)) & 0xFF00); } /** - * Unpack 32-bit data from a buffer in little-endian byte order - * @param buffer The buffer to unpack from - * @param offset The offset into he buffer to unpack - * @return The data that was unpacked - */ + * Unpack 32-bit data from a buffer in little-endian byte order + * @param buffer The buffer to unpack from + * @param offset The offset into he buffer to unpack + * @return The data that was unpacked + */ private static final int unpack32(byte[] buffer, int offset) { return ((int) buffer[offset] & 0xFF) | ((buffer[offset + 1] << 8) & 0xFF00) | ((buffer[offset + 2] << 16) & 0xFF0000) | ((buffer[offset + 3] << 24) & 0xFF000000);