/**
* 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.
* @param d The differential gain of the Jaguar's PID controller.
*/
public void setPID(double p, double i, double d) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize;
switch (m_controlMode.value) {
case ControlMode.kPercentVbus_val:
case ControlMode.kVoltage_val:
// TODO: Error, Not Valid
break;
case ControlMode.kSpeed_val:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(JaguarCANProtocol.LM_API_SPD_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(JaguarCANProtocol.LM_API_SPD_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(JaguarCANProtocol.LM_API_SPD_DC, dataBuffer, dataSize);
break;
case ControlMode.kPosition_val:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(JaguarCANProtocol.LM_API_POS_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(JaguarCANProtocol.LM_API_POS_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(JaguarCANProtocol.LM_API_POS_DC, dataBuffer, dataSize);
break;
case ControlMode.kCurrent_val:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(JaguarCANProtocol.LM_API_ICTRL_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(JaguarCANProtocol.LM_API_ICTRL_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(JaguarCANProtocol.LM_API_ICTRL_DC, dataBuffer, dataSize);
break;
}
}
/**
* Configure Soft Position Limits when in Position Controller mode.
*
* <p>When controlling position, you can add additional limits on top of the limit switch inputs
* that are based on the position feedback. If the position limit is reached or the switch is
* opened, that direction will be disabled.
*
* @param forwardLimitPosition The position that if exceeded will disable the forward direction.
* @param reverseLimitPosition The position that if exceeded will disable the reverse direction.
*/
public void configSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition)
throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize;
dataSize = packFXP16_16(dataBuffer, forwardLimitPosition);
dataBuffer[dataSize++] = (forwardLimitPosition > reverseLimitPosition) ? (byte) 1 : (byte) 0;
setTransaction(JaguarCANProtocol.LM_API_CFG_LIMIT_FWD, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, reverseLimitPosition);
dataBuffer[dataSize++] = forwardLimitPosition <= reverseLimitPosition ? (byte) 1 : (byte) 0;
setTransaction(JaguarCANProtocol.LM_API_CFG_LIMIT_REV, dataBuffer, dataSize);
dataBuffer[0] = LimitMode.kSoftPositionLimit_val;
setTransaction(JaguarCANProtocol.LM_API_CFG_LIMIT_MODE, dataBuffer, (byte) 1);
}
/**
* Configure the number of turns on the potentiometer.
*
* <p>There is no special support for continuous turn potentiometers. Only integer numbers of
* turns are supported.
*
* @param turns The number of turns of the potentiometer
*/
public void configPotentiometerTurns(int turns) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize;
dataSize = packINT16(dataBuffer, (short) turns);
setTransaction(JaguarCANProtocol.LM_API_CFG_POT_TURNS, dataBuffer, dataSize);
}
/**
* Configure how many codes per revolution are generated by your encoder.
*
* @param codesPerRev The number of counts per revolution in 1X mode.
*/
public void configEncoderCodesPerRev(int codesPerRev) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize;
dataSize = packINT16(dataBuffer, (short) codesPerRev);
setTransaction(JaguarCANProtocol.LM_API_CFG_ENC_LINES, dataBuffer, dataSize);
}
/**
* Configure how long the Jaguar waits in the case of a fault before resuming operation.
*
* <p>Faults include over temerature, over current, and bus under voltage. The default is 3.0
* seconds, but can be reduced to as low as 0.5 seconds.
*
* @param faultTime The time to wait before resuming operation, in seconds.
*/
public void configFaultTime(double faultTime) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize;
// Message takes ms
dataSize = packINT16(dataBuffer, (short) (faultTime * 1000.0));
setTransaction(JaguarCANProtocol.LM_API_CFG_FAULT_TIME, dataBuffer, dataSize);
}
/**
* Configure the maximum voltage that the Jaguar will ever output.
*
* <p>This can be used to limit the maximum output voltage in all modes so that motors which
* cannot withstand full bus voltage can be used safely.
*
* @param voltage The maximum voltage output by the Jaguar.
*/
public void configMaxOutputVoltage(double voltage) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize;
m_maxOutputVoltage = voltage;
dataSize = packFXP8_8(dataBuffer, voltage);
setTransaction(JaguarCANProtocol.LM_API_CFG_MAX_VOUT, dataBuffer, dataSize);
}
/**
* Set the maximum voltage change rate.
*
* <p>When in percent 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.
*
* @param rampRate The maximum rate of voltage change in Percent Voltage mode in V/s.
*/
public void setVoltageRampRate(double rampRate) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize;
switch (m_controlMode.value) {
case ControlMode.kPercentVbus_val:
dataSize = packPercentage(dataBuffer, rampRate / (m_maxOutputVoltage * kControllerRate));
setTransaction(JaguarCANProtocol.LM_API_VOLT_SET_RAMP, dataBuffer, dataSize);
break;
case ControlMode.kVoltage_val:
dataSize = packFXP8_8(dataBuffer, rampRate / kControllerRate);
setTransaction(JaguarCANProtocol.LM_API_VCOMP_IN_RAMP, dataBuffer, dataSize);
break;
default:
return;
}
}
/**
* Disable the closed loop controller.
*
* <p>Stop driving the output based on the feedback.
*/
public void disableControl() throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize = 0;
switch (m_controlMode.value) {
case ControlMode.kPercentVbus_val:
setTransaction(JaguarCANProtocol.LM_API_VOLT_DIS, dataBuffer, dataSize);
break;
case ControlMode.kSpeed_val:
setTransaction(JaguarCANProtocol.LM_API_SPD_DIS, dataBuffer, dataSize);
break;
case ControlMode.kPosition_val:
setTransaction(JaguarCANProtocol.LM_API_POS_DIS, dataBuffer, dataSize);
break;
case ControlMode.kCurrent_val:
setTransaction(JaguarCANProtocol.LM_API_ICTRL_DIS, dataBuffer, dataSize);
break;
case ControlMode.kVoltage_val:
setTransaction(JaguarCANProtocol.LM_API_VCOMP_DIS, dataBuffer, dataSize);
break;
}
}
/**
* Set the output set-point value.
*
* <p>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.
*/
public void setX(double outputValue, byte syncGroup) throws CANTimeoutException {
int messageID = 0;
byte[] dataBuffer = new byte[8];
byte dataSize = 0;
if (!m_safetyHelper.isAlive()) {
enableControl();
}
switch (m_controlMode.value) {
case ControlMode.kPercentVbus_val:
messageID = JaguarCANProtocol.LM_API_VOLT_T_SET;
if (outputValue > 1.0) outputValue = 1.0;
if (outputValue < -1.0) outputValue = -1.0;
packPercentage(dataBuffer, outputValue);
dataSize = 2;
break;
case ControlMode.kSpeed_val:
{
messageID = JaguarCANProtocol.LM_API_SPD_T_SET;
dataSize = packFXP16_16(dataBuffer, outputValue);
}
break;
case ControlMode.kPosition_val:
{
messageID = JaguarCANProtocol.LM_API_POS_T_SET;
dataSize = packFXP16_16(dataBuffer, outputValue);
}
break;
case ControlMode.kCurrent_val:
{
messageID = JaguarCANProtocol.LM_API_ICTRL_T_SET;
dataSize = packFXP8_8(dataBuffer, outputValue);
}
break;
case ControlMode.kVoltage_val:
{
messageID = JaguarCANProtocol.LM_API_VCOMP_T_SET;
dataSize = packFXP8_8(dataBuffer, outputValue);
}
break;
default:
return;
}
if (syncGroup != 0) {
dataBuffer[dataSize] = syncGroup;
dataSize++;
}
setTransaction(messageID, dataBuffer, dataSize);
m_safetyHelper.feed();
}
/**
* Enable the closed loop controller.
*
* <p>Start actually controlling the output based on the feedback. If starting a position
* controller with an encoder reference, use the encoderInitialPosition parameter to initialize
* the encoder state.
*
* @param encoderInitialPosition Encoder position to set if position with encoder reference.
* Ignored otherwise.
*/
public void enableControl(double encoderInitialPosition) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize = 0;
switch (m_controlMode.value) {
case ControlMode.kPercentVbus_val:
setTransaction(JaguarCANProtocol.LM_API_VOLT_T_EN, dataBuffer, dataSize);
break;
case ControlMode.kSpeed_val:
setTransaction(JaguarCANProtocol.LM_API_SPD_T_EN, dataBuffer, dataSize);
break;
case ControlMode.kPosition_val:
dataSize = packFXP16_16(dataBuffer, encoderInitialPosition);
setTransaction(JaguarCANProtocol.LM_API_POS_T_EN, dataBuffer, dataSize);
break;
case ControlMode.kCurrent_val:
setTransaction(JaguarCANProtocol.LM_API_ICTRL_T_EN, dataBuffer, dataSize);
break;
case ControlMode.kVoltage_val:
setTransaction(JaguarCANProtocol.LM_API_VCOMP_T_EN, dataBuffer, dataSize);
break;
}
}
/**
* Check if the Jaguar's power has been cycled since this was last called.
*
* <p>This should return true the first time called after a Jaguar power up, and false after that.
*
* @return The Jaguar was power cycled since the last call to this function.
*/
public boolean getPowerCycled() throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
byte dataSize;
dataSize = getTransaction(JaguarCANProtocol.LM_API_STATUS_POWER, dataBuffer);
if (dataSize == 1) {
boolean powerCycled = dataBuffer[0] != 0;
// Clear the power cycled bit now that we've accessed it
if (powerCycled) {
dataBuffer[0] = 1;
setTransaction(JaguarCANProtocol.LM_API_STATUS_POWER, dataBuffer, (byte) 1);
}
return powerCycled;
}
return false;
}
/**
* Set the reference source device for position controller mode.
*
* <p>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.
*/
public void setPositionReference(PositionReference reference) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
dataBuffer[0] = reference.value;
setTransaction(JaguarCANProtocol.LM_API_POS_REF, dataBuffer, (byte) 1);
}
/**
* Disable Soft Position Limits if previously enabled.
*
* <p>Soft Position Limits are disabled by default.
*/
public void disableSoftPositionLimits() throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
dataBuffer[0] = LimitMode.kSwitchInputsOnly_val;
setTransaction(JaguarCANProtocol.LM_API_CFG_LIMIT_MODE, dataBuffer, (byte) 1);
}
/**
* Configure what the controller does to the H-Bridge when neutral (not driving the output).
*
* <p>This allows you to override the jumper configuration for brake or coast.
*
* @param mode Select to use the jumper setting or to override it to coast or brake.
*/
public void configNeutralMode(NeutralMode mode) throws CANTimeoutException {
byte[] dataBuffer = new byte[8];
dataBuffer[0] = mode.value;
setTransaction(JaguarCANProtocol.LM_API_CFG_BRAKE_COAST, dataBuffer, (byte) 1);
}
