diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java
index 0dc0f276d0..722b362822 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java
@@ -42,11 +42,11 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
private static class EncoderTag {};
/** Sets an encoder as the speed reference only.
Passed as the "tag" when setting the control mode.*/
public final static EncoderTag kEncoder = new EncoderTag();
-
+
private static class QuadEncoderTag {};
- /** Sets a quadrature encoder as the position and speed reference.
Passed as the "tag" when setting the control mode.*/
+ /** Sets a quadrature encoder as the position and speed reference.
Passed as the "tag" when setting the control mode.*/
public final static QuadEncoderTag kQuadEncoder = new QuadEncoderTag();
-
+
private static class PotentiometerTag {};
/** Sets a potentiometer as the position reference only.
Passed as the "tag" when setting the control mode. */
public final static PotentiometerTag kPotentiometer = new PotentiometerTag();
@@ -77,7 +77,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
/** Use the NeutralMode that is set by the jumper wire on the CAN device */
Jumper((byte)0),
/** Stop the motor's rotation by applying a force. */
- Brake((byte)1),
+ Brake((byte)1),
/** Do not attempt to stop the motor. Instead allow it to coast to a stop without applying resistance. */
Coast((byte)2);
@@ -110,7 +110,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
* @see CANJaguar#getReverseLimitOK()
*/
SwitchInputsOnly((byte)0),
- /**
+ /**
* Enables the soft position limits on the Jaguar.
* These will be used in addition to the limit switches. This does not disable the behavior
* of the limit switch input.
@@ -138,8 +138,8 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
/**
* Constructor for the CANJaguar device.
* By default the device is configured in Percent mode.
- * The control mode can be changed by calling one of the control modes listed below.
- *
+ * The control mode can be changed by calling one of the control modes listed below.
+ *
* @param deviceNumber The address of the Jaguar on the CAN bus.
* @see CANJaguar#setCurrentMode(double, double, double)
* @see CANJaguar#setCurrentMode(PotentiometerTag, double, double, double)
@@ -165,7 +165,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
throw new AllocationException(
"CANJaguar device " + e1.getMessage() + "(increment index by one)");
}
-
+
m_deviceNumber = (byte)deviceNumber;
m_controlMode = ControlMode.PercentVbus;
@@ -260,7 +260,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
CANJNI.FRCNetworkCommunicationCANSessionMuxSendMessage(messageID, null,
CANJNI.CAN_SEND_PERIOD_STOP_REPEATING, status.asIntBuffer());
-
+
configMaxOutputVoltage(kApproxBusVoltage);
}
@@ -283,14 +283,14 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
/**
* Sets 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).
* In voltage Mode, the outputValue is in volts.
* In current Mode, the outputValue is in amps.
* In speed mode, the outputValue is in rotations/minute.
* 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.
*/
@@ -346,14 +346,14 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
/**
* Sets 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).
* In voltage mode, the outputValue is in volts.
* In current mode, the outputValue is in amps.
* In speed mode, the outputValue is in rotations/minute.
* In position mode, the outputValue is in rotations.
- *
+ *
* @param value
* The set-point to sent to the motor controller.
*/
@@ -380,40 +380,60 @@ 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
- // Jaguar and mark everything as unverified.
+ // Mark everything as unverified
+ m_controlModeVerified = false;
+ m_speedRefVerified = false;
+ m_posRefVerified = false;
+ m_neutralModeVerified = false;
+ m_encoderCodesPerRevVerified = false;
+ m_potentiometerTurnsVerified = false;
+ m_forwardLimitVerified = false;
+ m_reverseLimitVerified = false;
+ m_limitModeVerified = false;
+ m_maxOutputVoltageVerified = false;
+ m_faultTimeVerified = false;
+
+ if(m_controlMode == ControlMode.PercentVbus || m_controlMode == ControlMode.Voltage) {
+ m_voltageRampRateVerified = false;
+ }
+ else {
+ m_pVerified = false;
+ m_iVerified = false;
+ m_dVerified = false;
+ }
+
+ // Verify periodic status messages again
+ m_receivedStatusMessage0 = false;
+ m_receivedStatusMessage1 = false;
+ m_receivedStatusMessage2 = false;
+
+ // Remove any old values from netcomms. Otherwise, parameters
+ // are incorrectly marked as verified based on stale messages.
+ int[] messages = new int[] {
+ CANJNI.LM_API_SPD_REF, CANJNI.LM_API_POS_REF,
+ CANJNI.LM_API_SPD_PC, CANJNI.LM_API_POS_PC,
+ CANJNI.LM_API_ICTRL_PC, CANJNI.LM_API_SPD_IC,
+ CANJNI.LM_API_POS_IC, CANJNI.LM_API_ICTRL_IC,
+ CANJNI.LM_API_SPD_DC, CANJNI.LM_API_POS_DC,
+ CANJNI.LM_API_ICTRL_DC, CANJNI.LM_API_CFG_ENC_LINES,
+ CANJNI.LM_API_CFG_POT_TURNS, CANJNI.LM_API_CFG_BRAKE_COAST,
+ CANJNI.LM_API_CFG_LIMIT_MODE, CANJNI.LM_API_CFG_LIMIT_REV,
+ CANJNI.LM_API_CFG_MAX_VOUT, CANJNI.LM_API_VOLT_SET_RAMP,
+ CANJNI.LM_API_VCOMP_COMP_RAMP, CANJNI.LM_API_CFG_FAULT_TIME,
+ CANJNI.LM_API_CFG_LIMIT_FWD
+ };
+
+ for(int message : messages) {
+ try {
+ getMessage(message, CANJNI.CAN_MSGID_FULL_M, data);
+ } catch (CANMessageNotFoundException e) {}
+ }
+
enableControl();
- setSpeedReference(m_speedReference);
- setPositionReference(m_positionReference);
- configNeutralMode(m_neutralMode);
- configEncoderCodesPerRev(m_encoderCodesPerRev);
- configPotentiometerTurns(m_potentiometerTurns);
-
- switch(m_controlMode) {
-
- }
-
- if(m_controlMode == ControlMode.Current ||
- m_controlMode == ControlMode.Speed ||
- m_controlMode == ControlMode.Position) {
- setPID(m_p, m_i, m_d);
- }
-
- if(m_limitMode == LimitMode.SoftPositionLimits) {
- configSoftPositionLimits(m_forwardLimit, m_reverseLimit);
- }
-
- configMaxOutputVoltage(m_maxOutputVoltage);
-
- if(m_controlMode == ControlMode.Voltage ||
- m_controlMode == ControlMode.PercentVbus) {
- setVoltageRampRate(m_voltageRampRate);
- }
-
- setupPeriodicStatus();
}
- } catch(CANMessageNotFoundException e) {}
-
+ } catch(CANMessageNotFoundException e) {
+ requestMessage(CANJNI.LM_API_STATUS_POWER);
+ }
// Verify that any recently set parameters are correct
if(!m_controlModeVerified) {
@@ -536,7 +556,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
m_iVerified = true;
} else {
// It's wrong - set it again
- setI(m_p);
+ setI(m_i);
}
} catch(CANMessageNotFoundException e) {
// Verification is needed but not available - request it again.
@@ -695,7 +715,10 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
double voltage = unpackFXP8_8(data);
- if(FXP8_EQ(voltage, m_maxOutputVoltage)) {
+ // The returned max output voltage is sometimes slightly higher
+ // or lower than what was sent. This should not trigger
+ // resending the message.
+ if(Math.abs(voltage - m_maxOutputVoltage) < 0.1) {
m_maxOutputVoltageVerified = true;
} else {
// It's wrong - set it again
@@ -745,6 +768,24 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
requestMessage(CANJNI.LM_API_VCOMP_COMP_RAMP);
}
}
+
+ if(!m_faultTimeVerified) {
+ try {
+ getMessage(CANJNI.LM_API_CFG_FAULT_TIME, CANJNI.CAN_MSGID_FULL_M, data);
+
+ int faultTime = unpackINT16(data);
+
+ if((int)(m_faultTime * 1000.0) == faultTime) {
+ m_faultTimeVerified = true;
+ } else {
+ //It's wrong - set it again
+ configFaultTime(m_faultTime);
+ }
+ } catch(CANMessageNotFoundException e) {
+ // Verification is needed but not available - request it again.
+ requestMessage(CANJNI.LM_API_CFG_FAULT_TIME);
+ }
+ }
}
/**
@@ -1205,7 +1246,7 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
/**
* Enable controlling the position with a feedback loop using a potentiometer.
* After calling this you must call {@link CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} to enable the device.
- *
+ *
* @param tag The constant {@link CANJaguar#kPotentiometer}
* @param p The proportional gain of the Jaguar's PID controller.
* @param i The integral gain of the Jaguar's PID controller.
@@ -1278,14 +1319,14 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
/**
* Used internally. In order to set the control mode see the methods listed below.
- *
+ *
* Change the control mode of this Jaguar object.
*
* After changing modes, configure any PID constants or other settings needed
* and then EnableControl() to actually change the mode on the Jaguar.
*
* @param controlMode The new mode.
- *
+ *
* @see CANJaguar#setCurrentMode(double, double, double)
* @see CANJaguar#setCurrentMode(PotentiometerTag, double, double, double)
* @see CANJaguar#setCurrentMode(EncoderTag, int, double, double, double)
@@ -1622,6 +1663,9 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
public void configFaultTime(float faultTime) {
byte[] data = new byte[8];
+ if(faultTime < 0.5f) faultTime = 0.5f;
+ else if(faultTime > 3.0f) faultTime = 3.0f;
+
int dataSize = packINT16(data, (short)(faultTime * 1000.0));
sendMessage(CANJNI.LM_API_CFG_FAULT_TIME, data, dataSize);