Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fixed some bugs with CANJaguar verification in Java

Browse Source 
Change-Id: I5f6510d53c806845f6bae5eb8fd9ebbc8fde054e
This commit is contained in:
thomasclark
2014-07-23 16:02:59 -04:00
parent c3d1e80a62
commit 66ba9a728e
1 changed files with 94 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

144
wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANJaguar.java
View File

@@ -42,11 +42,11 @@ public class CANJaguar implements MotorSafety, PIDOutput, SpeedController, LiveW
private static class EncoderTag {};
/** Sets an encoder as the speed reference only. <br> 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. <br> Passed as the "tag" when setting the control mode.*/
/** Sets a quadrature encoder as the position and speed reference. <br> 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. <br> 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.<br>
* 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.<br>
* In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM Jaguar).<br>
* In voltage Mode, the outputValue is in volts. <br>
* In current Mode, the outputValue is in amps.<br>
* In speed mode, the outputValue is in rotations/minute.<br>
* 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.<br>
* In percentVbus mode, the outputValue is from -1.0 to 1.0 (same as PWM Jaguar).<br>
* In voltage mode, the outputValue is in volts. <br>
* In current mode, the outputValue is in amps. <br>
* In speed mode, the outputValue is in rotations/minute.<br>
* 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.<br>
* 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);