/**
* 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).
* 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.
*/
void CANJaguar::Set(float outputValue, uint8_t syncGroup)
{
uint32_t messageID;
uint8_t dataBuffer[8];
uint8_t dataSize;
if (m_safetyHelper && !m_safetyHelper->IsAlive())
{
EnableControl();
}
switch(m_controlMode)
{
case kPercentVbus:
{
messageID = LM_API_VOLT_T_SET;
if (outputValue > 1.0) outputValue = 1.0;
if (outputValue < -1.0) outputValue = -1.0;
dataSize = packPercentage(dataBuffer, outputValue);
}
break;
case kSpeed:
{
messageID = LM_API_SPD_T_SET;
dataSize = packFXP16_16(dataBuffer, outputValue);
}
break;
case kPosition:
{
messageID = LM_API_POS_T_SET;
dataSize = packFXP16_16(dataBuffer, outputValue);
}
break;
case kCurrent:
{
messageID = LM_API_ICTRL_T_SET;
dataSize = packFXP8_8(dataBuffer, outputValue);
}
break;
case kVoltage:
{
messageID = LM_API_VCOMP_T_SET;
dataSize = packFXP8_8(dataBuffer, outputValue);
}
break;
default:
return;
}
if (syncGroup != 0)
{
dataBuffer[dataSize] = syncGroup;
dataSize++;
}
setTransaction(messageID, dataBuffer, dataSize);
if (m_safetyHelper) m_safetyHelper->Feed();
}
/**
* Enable the closed loop controller.
*
* 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.
*/
void CANJaguar::EnableControl(double encoderInitialPosition)
{
uint8_t dataBuffer[8];
uint8_t dataSize = 0;
switch(m_controlMode)
{
case kPercentVbus:
setTransaction(LM_API_VOLT_T_EN, dataBuffer, dataSize);
break;
case kSpeed:
setTransaction(LM_API_SPD_T_EN, dataBuffer, dataSize);
break;
case kPosition:
dataSize = packFXP16_16(dataBuffer, encoderInitialPosition);
setTransaction(LM_API_POS_T_EN, dataBuffer, dataSize);
break;
case kCurrent:
setTransaction(LM_API_ICTRL_T_EN, dataBuffer, dataSize);
break;
case kVoltage:
setTransaction(LM_API_VCOMP_T_EN, dataBuffer, dataSize);
break;
}
}
/**
* Configure Soft Position Limits when in Position Controller mode.
*
* 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.
*/
void CANJaguar::ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition)
{
uint8_t dataBuffer[8];
uint8_t dataSize;
dataSize = packFXP16_16(dataBuffer, forwardLimitPosition);
dataBuffer[dataSize++] = forwardLimitPosition > reverseLimitPosition;
setTransaction(LM_API_CFG_LIMIT_FWD, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, reverseLimitPosition);
dataBuffer[dataSize++] = forwardLimitPosition <= reverseLimitPosition;
setTransaction(LM_API_CFG_LIMIT_REV, dataBuffer, dataSize);
dataBuffer[0] = kLimitMode_SoftPositionLimits;
setTransaction(LM_API_CFG_LIMIT_MODE, dataBuffer, sizeof(uint8_t));
}
/**
* Set the output set-point value.
*
* In PercentVoltage Mode, the input is in the range -1.0 to 1.0
*
* @param outputValue The set-point to sent to the motor controller.
*/
void CANJaguar::Set(float outputValue)
{
UINT32 messageID;
UINT8 dataBuffer[8];
UINT8 dataSize;
switch(m_controlMode)
{
case kPercentVoltage:
{
messageID = LM_API_VOLT_T_SET;
dataSize = packPercentage(dataBuffer, outputValue);
}
break;
case kSpeed:
{
messageID = LM_API_SPD_T_SET;
dataSize = packFXP16_16(dataBuffer, outputValue);
}
break;
case kPosition:
{
messageID = LM_API_POS_T_SET;
dataSize = packFXP16_16(dataBuffer, outputValue);
}
break;
case kCurrent:
{
messageID = LM_API_ICTRL_T_SET;
dataSize = packFXP8_8(dataBuffer, outputValue);
}
break;
default:
return;
}
setTransaction(messageID, dataBuffer, dataSize);
}
/**
* 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.
*/
void CANJaguar::SetPID(double p, double i, double d)
{
uint8_t dataBuffer[8];
uint8_t dataSize;
switch(m_controlMode)
{
case kPercentVbus:
case kVoltage:
wpi_setWPIErrorWithContext(IncompatibleMode, "PID constants only apply in Speed, Position, and Current mode");
break;
case kSpeed:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(LM_API_SPD_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(LM_API_SPD_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(LM_API_SPD_DC, dataBuffer, dataSize);
break;
case kPosition:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(LM_API_POS_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(LM_API_POS_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(LM_API_POS_DC, dataBuffer, dataSize);
break;
case kCurrent:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(LM_API_ICTRL_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(LM_API_ICTRL_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(LM_API_ICTRL_DC, dataBuffer, dataSize);
break;
}
}
/**
* 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.
*/
void CANJaguar::SetPID(double p, double i, double d)
{
UINT8 dataBuffer[8];
UINT8 dataSize;
switch(m_controlMode)
{
case kPercentVbus:
case kVoltage:
// TODO: Error, Not Valid
break;
case kSpeed:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(LM_API_SPD_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(LM_API_SPD_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(LM_API_SPD_DC, dataBuffer, dataSize);
break;
case kPosition:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(LM_API_POS_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(LM_API_POS_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(LM_API_POS_DC, dataBuffer, dataSize);
break;
case kCurrent:
dataSize = packFXP16_16(dataBuffer, p);
setTransaction(LM_API_ICTRL_PC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, i);
setTransaction(LM_API_ICTRL_IC, dataBuffer, dataSize);
dataSize = packFXP16_16(dataBuffer, d);
setTransaction(LM_API_ICTRL_DC, dataBuffer, dataSize);
break;
}
}
/**
* Enable the closed loop controller.
*
* 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.
*/
void CANJaguar::EnableControl(double encoderInitialPosition)
{
UINT8 dataBuffer[8];
UINT8 dataSize = 0;
switch(m_controlMode)
{
case kPercentVoltage:
// TODO: Error, Not Valid
break;
case kSpeed:
setTransaction(LM_API_SPD_T_EN, dataBuffer, dataSize);
break;
case kPosition:
dataSize = packFXP16_16(dataBuffer, encoderInitialPosition);
setTransaction(LM_API_POS_T_EN, dataBuffer, dataSize);
break;
case kCurrent:
setTransaction(LM_API_ICTRL_T_EN, dataBuffer, dataSize);
break;
}
}
