/** * 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; } }