Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/Athena/src/CANJaguar.cpp b/wpilibc/Athena/src/CANJaguar.cpp
new file mode 100644
index 0000000..fc71e84
--- /dev/null
+++ b/wpilibc/Athena/src/CANJaguar.cpp
@@ -0,0 +1,2013 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2009. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "CANJaguar.h"
+#include "Timer.h"
+#define tNIRIO_i32 int
+#include "NetworkCommunication/CANSessionMux.h"
+#include "WPIErrors.h"
+#include <cstdio>
+#include <cassert>
+#include "LiveWindow/LiveWindow.h"
+
+/* we are on ARM-LE now, not Freescale so no need to swap */
+#define swap16(x) (x)
+#define swap32(x) (x)
+
+/* Compare floats for equality as fixed point numbers */
+#define FXP8_EQ(a, b) ((int16_t)((a)*256.0) == (int16_t)((b)*256.0))
+#define FXP16_EQ(a, b) ((int32_t)((a)*65536.0) == (int32_t)((b)*65536.0))
+
+const int32_t CANJaguar::kControllerRate;
+constexpr double CANJaguar::kApproxBusVoltage;
+
+static const int32_t kSendMessagePeriod = 20;
+static const uint32_t kFullMessageIDMask =
+ (CAN_MSGID_API_M | CAN_MSGID_MFR_M | CAN_MSGID_DTYPE_M);
+
+static const int32_t kReceiveStatusAttempts = 50;
+
+static std::unique_ptr<Resource> allocated;
+
+static int32_t sendMessageHelper(uint32_t messageID, const uint8_t *data,
+ uint8_t dataSize, int32_t period) {
+ static const uint32_t kTrustedMessages[] = {
+ LM_API_VOLT_T_EN, LM_API_VOLT_T_SET, LM_API_SPD_T_EN, LM_API_SPD_T_SET,
+ LM_API_VCOMP_T_EN, LM_API_VCOMP_T_SET, LM_API_POS_T_EN, LM_API_POS_T_SET,
+ LM_API_ICTRL_T_EN, LM_API_ICTRL_T_SET};
+
+ int32_t status = 0;
+
+ for (auto& kTrustedMessage : kTrustedMessages) {
+ if ((kFullMessageIDMask & messageID) == kTrustedMessage) {
+ uint8_t dataBuffer[8];
+ dataBuffer[0] = 0;
+ dataBuffer[1] = 0;
+
+ // Make sure the data will still fit after adjusting for the token.
+ assert(dataSize <= 6);
+
+ for (uint8_t j = 0; j < dataSize; j++) {
+ dataBuffer[j + 2] = data[j];
+ }
+
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ messageID, dataBuffer, dataSize + 2, period, &status);
+
+ return status;
+ }
+ }
+
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(messageID, data, dataSize,
+ period, &status);
+
+ return status;
+}
+
+/**
+ * Common initialization code called by all constructors.
+ */
+void CANJaguar::InitCANJaguar() {
+ m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+
+ bool receivedFirmwareVersion = false;
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ // Request firmware and hardware version only once
+ requestMessage(CAN_IS_FRAME_REMOTE | CAN_MSGID_API_FIRMVER);
+ requestMessage(LM_API_HWVER);
+
+ // Wait until we've gotten all of the status data at least once.
+ for (int i = 0; i < kReceiveStatusAttempts; i++) {
+ Wait(0.001);
+
+ setupPeriodicStatus();
+ updatePeriodicStatus();
+
+ if (!receivedFirmwareVersion &&
+ getMessage(CAN_MSGID_API_FIRMVER, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ m_firmwareVersion = unpackint32_t(dataBuffer);
+ receivedFirmwareVersion = true;
+ }
+
+ if (m_receivedStatusMessage0 && m_receivedStatusMessage1 &&
+ m_receivedStatusMessage2 && receivedFirmwareVersion) {
+ break;
+ }
+ }
+
+ if (!m_receivedStatusMessage0 || !m_receivedStatusMessage1 ||
+ !m_receivedStatusMessage2 || !receivedFirmwareVersion) {
+ wpi_setWPIErrorWithContext(JaguarMessageNotFound, "Status data not found");
+ }
+
+ if (getMessage(LM_API_HWVER, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
+ m_hardwareVersion = dataBuffer[0];
+
+ if (m_deviceNumber < 1 || m_deviceNumber > 63) {
+ std::stringstream buf;
+ buf << "device number \"" << m_deviceNumber
+ << "\" must be between 1 and 63";
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, buf.str());
+ return;
+ }
+
+ if (StatusIsFatal()) return;
+
+ // 3330 was the first shipping RDK firmware version for the Jaguar
+ if (m_firmwareVersion >= 3330 || m_firmwareVersion < 108) {
+ std::stringstream buf;
+ if (m_firmwareVersion < 3330) {
+ buf << "Jag #" << m_deviceNumber << " firmware (" << m_firmwareVersion
+ << ") is too old (must be at least version 108 "
+ "of the FIRST approved firmware)";
+ } else {
+ buf << "Jag #" << m_deviceNumber << " firmware (" << m_firmwareVersion
+ << ") is not FIRST approved (must be at least "
+ "version 108 of the FIRST approved firmware)";
+ }
+ wpi_setWPIErrorWithContext(JaguarVersionError, buf.str());
+ return;
+ }
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ case kVoltage:
+ // No additional configuration required... start enabled.
+ EnableControl();
+ break;
+ default:
+ break;
+ }
+ HALReport(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber,
+ m_controlMode);
+ LiveWindow::GetInstance()->AddActuator("CANJaguar", m_deviceNumber, this);
+}
+
+/**
+ * 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.
+ *
+ * @param deviceNumber The address of the Jaguar on the CAN bus.
+ * @see CANJaguar#SetCurrentMode(double, double, double)
+ * @see CANJaguar#SetCurrentMode(PotentiometerTag, double, double, double)
+ * @see CANJaguar#SetCurrentMode(EncoderTag, int, double, double, double)
+ * @see CANJaguar#SetCurrentMode(QuadEncoderTag, int, double, double, double)
+ * @see CANJaguar#SetPercentMode()
+ * @see CANJaguar#SetPercentMode(PotentiometerTag)
+ * @see CANJaguar#SetPercentMode(EncoderTag, int)
+ * @see CANJaguar#SetPercentMode(QuadEncoderTag, int)
+ * @see CANJaguar#SetPositionMode(PotentiometerTag, double, double, double)
+ * @see CANJaguar#SetPositionMode(QuadEncoderTag, int, double, double, double)
+ * @see CANJaguar#SetSpeedMode(EncoderTag, int, double, double, double)
+ * @see CANJaguar#SetSpeedMode(QuadEncoderTag, int, double, double, double)
+ * @see CANJaguar#SetVoltageMode()
+ * @see CANJaguar#SetVoltageMode(PotentiometerTag)
+ * @see CANJaguar#SetVoltageMode(EncoderTag, int)
+ * @see CANJaguar#SetVoltageMode(QuadEncoderTag, int)
+ */
+CANJaguar::CANJaguar(uint8_t deviceNumber)
+ : m_deviceNumber(deviceNumber) {
+ std::stringstream buf;
+ buf << "CANJaguar device number " << m_deviceNumber;
+ Resource::CreateResourceObject(allocated, 63);
+
+ if (allocated->Allocate(m_deviceNumber - 1, buf.str()) ==
+ std::numeric_limits<uint32_t>::max()) {
+ CloneError(*allocated);
+ return;
+ }
+
+ SetPercentMode();
+ InitCANJaguar();
+ ConfigMaxOutputVoltage(kApproxBusVoltage);
+}
+
+CANJaguar::~CANJaguar() {
+ allocated->Free(m_deviceNumber - 1);
+
+ int32_t status;
+
+ // Disable periodic setpoints
+ if (m_controlMode == kPercentVbus)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_VOLT_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+ else if (m_controlMode == kSpeed)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_SPD_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+ else if (m_controlMode == kPosition)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_POS_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+ else if (m_controlMode == kCurrent)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_ICTRL_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+ else if (m_controlMode == kVoltage)
+ FRC_NetworkCommunication_CANSessionMux_sendMessage(
+ m_deviceNumber | LM_API_VCOMP_T_SET, nullptr, 0,
+ CAN_SEND_PERIOD_STOP_REPEATING, &status);
+
+ if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * @return The CAN ID passed in the constructor
+ */
+uint8_t CANJaguar::getDeviceNumber() const { return m_deviceNumber; }
+
+/**
+ * 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.
+ */
+void CANJaguar::Set(float outputValue, uint8_t syncGroup) {
+ uint32_t messageID;
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ if (m_safetyHelper && !m_safetyHelper->IsAlive() && m_controlEnabled) {
+ EnableControl();
+ }
+
+ if (m_controlEnabled) {
+ 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,
+ (m_isInverted ? -outputValue : outputValue));
+ } break;
+ case kSpeed: {
+ messageID = LM_API_SPD_T_SET;
+ dataSize = packFXP16_16(dataBuffer,
+ (m_isInverted ? -outputValue : 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, (m_isInverted ? -outputValue : outputValue));
+ } break;
+ default:
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "The Jaguar only supports Current, Voltage, "
+ "Position, Speed, and Percent (Throttle) "
+ "modes.");
+ return;
+ }
+ if (syncGroup != 0) {
+ dataBuffer[dataSize] = syncGroup;
+ dataSize++;
+ }
+
+ sendMessage(messageID, dataBuffer, dataSize, kSendMessagePeriod);
+
+ if (m_safetyHelper) m_safetyHelper->Feed();
+ }
+
+ m_value = outputValue;
+
+ verify();
+}
+
+/**
+ * Get the recently set outputValue setpoint.
+ *
+ * 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.<br>
+ *
+ * @return The most recently set outputValue setpoint.
+ */
+float CANJaguar::Get() const { return m_value; }
+
+/**
+* Common interface for disabling a motor.
+*
+* @deprecated Call {@link #DisableControl()} instead.
+*/
+void CANJaguar::Disable() { DisableControl(); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @deprecated Call Set instead.
+ *
+ * @param output Write out the PercentVbus value as was computed by the
+ * PIDController
+ */
+void CANJaguar::PIDWrite(float output) {
+ if (m_controlMode == kPercentVbus) {
+ Set(output);
+ } else {
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "PID only supported in PercentVbus mode");
+ }
+}
+
+uint8_t CANJaguar::packPercentage(uint8_t *buffer, double value) {
+ int16_t intValue = (int16_t)(value * 32767.0);
+ *((int16_t *)buffer) = swap16(intValue);
+ return sizeof(int16_t);
+}
+
+uint8_t CANJaguar::packFXP8_8(uint8_t *buffer, double value) {
+ int16_t intValue = (int16_t)(value * 256.0);
+ *((int16_t *)buffer) = swap16(intValue);
+ return sizeof(int16_t);
+}
+
+uint8_t CANJaguar::packFXP16_16(uint8_t *buffer, double value) {
+ int32_t intValue = (int32_t)(value * 65536.0);
+ *((int32_t *)buffer) = swap32(intValue);
+ return sizeof(int32_t);
+}
+
+uint8_t CANJaguar::packint16_t(uint8_t *buffer, int16_t value) {
+ *((int16_t *)buffer) = swap16(value);
+ return sizeof(int16_t);
+}
+
+uint8_t CANJaguar::packint32_t(uint8_t *buffer, int32_t value) {
+ *((int32_t *)buffer) = swap32(value);
+ return sizeof(int32_t);
+}
+
+double CANJaguar::unpackPercentage(uint8_t *buffer) const {
+ int16_t value = *((int16_t *)buffer);
+ value = swap16(value);
+ return value / 32767.0;
+}
+
+double CANJaguar::unpackFXP8_8(uint8_t *buffer) const {
+ int16_t value = *((int16_t *)buffer);
+ value = swap16(value);
+ return value / 256.0;
+}
+
+double CANJaguar::unpackFXP16_16(uint8_t *buffer) const {
+ int32_t value = *((int32_t *)buffer);
+ value = swap32(value);
+ return value / 65536.0;
+}
+
+int16_t CANJaguar::unpackint16_t(uint8_t *buffer) const {
+ int16_t value = *((int16_t *)buffer);
+ return swap16(value);
+}
+
+int32_t CANJaguar::unpackint32_t(uint8_t *buffer) const {
+ int32_t value = *((int32_t *)buffer);
+ return swap32(value);
+}
+
+/**
+ * Send a message to the Jaguar.
+ *
+ * @param messageID The messageID to be used on the CAN bus (device number is
+ * added internally)
+ * @param data The up to 8 bytes of data to be sent with the message
+ * @param dataSize Specify how much of the data in "data" to send
+ * @param periodic If positive, tell Network Communications to send the message
+ * every "period" milliseconds.
+ */
+void CANJaguar::sendMessage(uint32_t messageID, const uint8_t *data,
+ uint8_t dataSize, int32_t period) {
+ int32_t localStatus =
+ sendMessageHelper(messageID | m_deviceNumber, data, dataSize, period);
+
+ if (localStatus < 0) {
+ wpi_setErrorWithContext(localStatus, "sendMessage");
+ }
+}
+
+/**
+ * Request a message from the Jaguar, but don't wait for it to arrive.
+ *
+ * @param messageID The message to request
+ * @param periodic If positive, tell Network Communications to send the message
+ * every "period" milliseconds.
+ */
+void CANJaguar::requestMessage(uint32_t messageID, int32_t period) {
+ sendMessageHelper(messageID | m_deviceNumber, nullptr, 0, period);
+}
+
+/**
+ * Get a previously requested message.
+ *
+ * Jaguar always generates a message with the same message ID when replying.
+ *
+ * @param messageID The messageID to read from the CAN bus (device number is
+ * added internally)
+ * @param data The up to 8 bytes of data that was received with the message
+ * @param dataSize Indicates how much data was received
+ *
+ * @return true if the message was found. Otherwise, no new message is
+ * available.
+ */
+bool CANJaguar::getMessage(uint32_t messageID, uint32_t messageMask,
+ uint8_t *data, uint8_t *dataSize) const {
+ uint32_t targetedMessageID = messageID | m_deviceNumber;
+ int32_t status = 0;
+ uint32_t timeStamp;
+
+ // Caller may have set bit31 for remote frame transmission so clear invalid
+ // bits[31-29]
+ targetedMessageID &= CAN_MSGID_FULL_M;
+
+ // Get the data.
+ FRC_NetworkCommunication_CANSessionMux_receiveMessage(
+ &targetedMessageID, messageMask, data, dataSize, &timeStamp, &status);
+
+ // Do we already have the most recent value?
+ if (status == ERR_CANSessionMux_MessageNotFound)
+ return false;
+ else
+ wpi_setErrorWithContext(status, "receiveMessage");
+
+ return true;
+}
+
+/**
+ * Enables periodic status updates from the Jaguar.
+ */
+void CANJaguar::setupPeriodicStatus() {
+ uint8_t data[8];
+ uint8_t dataSize;
+
+ // Message 0 returns bus voltage, output voltage, output current, and
+ // temperature.
+ static const uint8_t kMessage0Data[] = {
+ LM_PSTAT_VOLTBUS_B0, LM_PSTAT_VOLTBUS_B1, LM_PSTAT_VOLTOUT_B0,
+ LM_PSTAT_VOLTOUT_B1, LM_PSTAT_CURRENT_B0, LM_PSTAT_CURRENT_B1,
+ LM_PSTAT_TEMP_B0, LM_PSTAT_TEMP_B1};
+
+ // Message 1 returns position and speed
+ static const uint8_t kMessage1Data[] = {
+ LM_PSTAT_POS_B0, LM_PSTAT_POS_B1, LM_PSTAT_POS_B2, LM_PSTAT_POS_B3,
+ LM_PSTAT_SPD_B0, LM_PSTAT_SPD_B1, LM_PSTAT_SPD_B2, LM_PSTAT_SPD_B3};
+
+ // Message 2 returns limits and faults
+ static const uint8_t kMessage2Data[] = {LM_PSTAT_LIMIT_CLR, LM_PSTAT_FAULT,
+ LM_PSTAT_END};
+
+ dataSize = packint16_t(data, kSendMessagePeriod);
+ sendMessage(LM_API_PSTAT_PER_EN_S0, data, dataSize);
+ sendMessage(LM_API_PSTAT_PER_EN_S1, data, dataSize);
+ sendMessage(LM_API_PSTAT_PER_EN_S2, data, dataSize);
+
+ dataSize = 8;
+ sendMessage(LM_API_PSTAT_CFG_S0, kMessage0Data, dataSize);
+ sendMessage(LM_API_PSTAT_CFG_S1, kMessage1Data, dataSize);
+ sendMessage(LM_API_PSTAT_CFG_S2, kMessage2Data, dataSize);
+}
+
+/**
+ * Check for new periodic status updates and unpack them into local variables
+ */
+void CANJaguar::updatePeriodicStatus() const {
+ uint8_t data[8];
+ uint8_t dataSize;
+
+ // Check if a new bus voltage/output voltage/current/temperature message
+ // has arrived and unpack the values into the cached member variables
+ if (getMessage(LM_API_PSTAT_DATA_S0, CAN_MSGID_FULL_M, data, &dataSize)) {
+ m_mutex.lock();
+ m_busVoltage = unpackFXP8_8(data);
+ m_outputVoltage = unpackPercentage(data + 2) * m_busVoltage;
+ m_outputCurrent = unpackFXP8_8(data + 4);
+ m_temperature = unpackFXP8_8(data + 6);
+ m_mutex.unlock();
+
+ m_receivedStatusMessage0 = true;
+ }
+
+ // Check if a new position/speed message has arrived and do the same
+ if (getMessage(LM_API_PSTAT_DATA_S1, CAN_MSGID_FULL_M, data, &dataSize)) {
+ m_mutex.lock();
+ m_position = unpackFXP16_16(data);
+ m_speed = unpackFXP16_16(data + 4);
+ m_mutex.unlock();
+
+ m_receivedStatusMessage1 = true;
+ }
+
+ // Check if a new limits/faults message has arrived and do the same
+ if (getMessage(LM_API_PSTAT_DATA_S2, CAN_MSGID_FULL_M, data, &dataSize)) {
+ m_mutex.lock();
+ m_limits = data[0];
+ m_faults = data[1];
+ m_mutex.unlock();
+
+ m_receivedStatusMessage2 = true;
+ }
+}
+
+/**
+ * Check all unverified params and make sure they're equal to their local
+ * cached versions. If a value isn't available, it gets requested. If a value
+ * doesn't match up, it gets set again.
+ */
+void CANJaguar::verify() {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ // If the Jaguar lost power, everything should be considered unverified.
+ if (getMessage(LM_API_STATUS_POWER, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ bool powerCycled = (bool)dataBuffer[0];
+
+ if (powerCycled) {
+ // Clear the power cycled bit
+ dataBuffer[0] = 1;
+ sendMessage(LM_API_STATUS_POWER, dataBuffer, sizeof(uint8_t));
+
+ // 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 == kPercentVbus || m_controlMode == kVoltage) {
+ 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.
+ getMessage(LM_API_SPD_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_POS_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_SPD_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_POS_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_ICTRL_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_SPD_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_POS_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_ICTRL_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_SPD_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_POS_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_ICTRL_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_BRAKE_COAST, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize);
+ getMessage(LM_API_CFG_ENC_LINES, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_POT_TURNS, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_LIMIT_MODE, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize);
+ getMessage(LM_API_CFG_LIMIT_FWD, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_LIMIT_REV, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_CFG_MAX_VOUT, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_VOLT_SET_RAMP, CAN_MSGID_FULL_M, dataBuffer, &dataSize);
+ getMessage(LM_API_VCOMP_COMP_RAMP, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize);
+ getMessage(LM_API_CFG_FAULT_TIME, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize);
+ }
+ } else {
+ requestMessage(LM_API_STATUS_POWER);
+ }
+
+ // Verify that any recently set parameters are correct
+ if (!m_controlModeVerified && m_controlEnabled) {
+ if (getMessage(LM_API_STATUS_CMODE, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ ControlMode mode = (ControlMode)dataBuffer[0];
+
+ if (m_controlMode == mode)
+ m_controlModeVerified = true;
+ else
+ // Enable control again to resend the control mode
+ EnableControl();
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_STATUS_CMODE);
+ }
+ }
+
+ if (!m_speedRefVerified) {
+ if (getMessage(LM_API_SPD_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ uint8_t speedRef = dataBuffer[0];
+
+ if (m_speedReference == speedRef)
+ m_speedRefVerified = true;
+ else
+ // It's wrong - set it again
+ SetSpeedReference(m_speedReference);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_SPD_REF);
+ }
+ }
+
+ if (!m_posRefVerified) {
+ if (getMessage(LM_API_POS_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ uint8_t posRef = dataBuffer[0];
+
+ if (m_positionReference == posRef)
+ m_posRefVerified = true;
+ else
+ // It's wrong - set it again
+ SetPositionReference(m_positionReference);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_POS_REF);
+ }
+ }
+
+ if (!m_pVerified) {
+ uint32_t message = 0;
+
+ if (m_controlMode == kSpeed)
+ message = LM_API_SPD_PC;
+ else if (m_controlMode == kPosition)
+ message = LM_API_POS_PC;
+ else if (m_controlMode == kCurrent)
+ message = LM_API_ICTRL_PC;
+ else {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return;
+ }
+
+ if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ double p = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(m_p, p))
+ m_pVerified = true;
+ else
+ // It's wrong - set it again
+ SetP(m_p);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(message);
+ }
+ }
+
+ if (!m_iVerified) {
+ uint32_t message = 0;
+
+ if (m_controlMode == kSpeed)
+ message = LM_API_SPD_IC;
+ else if (m_controlMode == kPosition)
+ message = LM_API_POS_IC;
+ else if (m_controlMode == kCurrent)
+ message = LM_API_ICTRL_IC;
+ else {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return;
+ }
+
+ if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ double i = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(m_i, i))
+ m_iVerified = true;
+ else
+ // It's wrong - set it again
+ SetI(m_i);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(message);
+ }
+ }
+
+ if (!m_dVerified) {
+ uint32_t message = 0;
+
+ if (m_controlMode == kSpeed)
+ message = LM_API_SPD_DC;
+ else if (m_controlMode == kPosition)
+ message = LM_API_POS_DC;
+ else if (m_controlMode == kCurrent)
+ message = LM_API_ICTRL_DC;
+ else {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return;
+ }
+
+ if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) {
+ double d = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(m_d, d))
+ m_dVerified = true;
+ else
+ // It's wrong - set it again
+ SetD(m_d);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(message);
+ }
+ }
+
+ if (!m_neutralModeVerified) {
+ if (getMessage(LM_API_CFG_BRAKE_COAST, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ NeutralMode mode = (NeutralMode)dataBuffer[0];
+
+ if (mode == m_neutralMode)
+ m_neutralModeVerified = true;
+ else
+ // It's wrong - set it again
+ ConfigNeutralMode(m_neutralMode);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_BRAKE_COAST);
+ }
+ }
+
+ if (!m_encoderCodesPerRevVerified) {
+ if (getMessage(LM_API_CFG_ENC_LINES, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ uint16_t codes = unpackint16_t(dataBuffer);
+
+ if (codes == m_encoderCodesPerRev)
+ m_encoderCodesPerRevVerified = true;
+ else
+ // It's wrong - set it again
+ ConfigEncoderCodesPerRev(m_encoderCodesPerRev);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_ENC_LINES);
+ }
+ }
+
+ if (!m_potentiometerTurnsVerified) {
+ if (getMessage(LM_API_CFG_POT_TURNS, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ uint16_t turns = unpackint16_t(dataBuffer);
+
+ if (turns == m_potentiometerTurns)
+ m_potentiometerTurnsVerified = true;
+ else
+ // It's wrong - set it again
+ ConfigPotentiometerTurns(m_potentiometerTurns);
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_POT_TURNS);
+ }
+ }
+
+ if (!m_limitModeVerified) {
+ if (getMessage(LM_API_CFG_LIMIT_MODE, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ LimitMode mode = (LimitMode)dataBuffer[0];
+
+ if (mode == m_limitMode)
+ m_limitModeVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigLimitMode(m_limitMode);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_LIMIT_MODE);
+ }
+ }
+
+ if (!m_forwardLimitVerified) {
+ if (getMessage(LM_API_CFG_LIMIT_FWD, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double limit = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(limit, m_forwardLimit))
+ m_forwardLimitVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigForwardLimit(m_forwardLimit);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_LIMIT_FWD);
+ }
+ }
+
+ if (!m_reverseLimitVerified) {
+ if (getMessage(LM_API_CFG_LIMIT_REV, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double limit = unpackFXP16_16(dataBuffer);
+
+ if (FXP16_EQ(limit, m_reverseLimit))
+ m_reverseLimitVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigReverseLimit(m_reverseLimit);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_LIMIT_REV);
+ }
+ }
+
+ if (!m_maxOutputVoltageVerified) {
+ if (getMessage(LM_API_CFG_MAX_VOUT, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double voltage = unpackFXP8_8(dataBuffer);
+
+ // The returned max output voltage is sometimes slightly higher or
+ // lower than what was sent. This should not trigger resending
+ // the message.
+ if (std::abs(voltage - m_maxOutputVoltage) < 0.1)
+ m_maxOutputVoltageVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigMaxOutputVoltage(m_maxOutputVoltage);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_MAX_VOUT);
+ }
+ }
+
+ if (!m_voltageRampRateVerified) {
+ if (m_controlMode == kPercentVbus) {
+ if (getMessage(LM_API_VOLT_SET_RAMP, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double rate = unpackPercentage(dataBuffer);
+
+ if (FXP16_EQ(rate, m_voltageRampRate))
+ m_voltageRampRateVerified = true;
+ else {
+ // It's wrong - set it again
+ SetVoltageRampRate(m_voltageRampRate);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_VOLT_SET_RAMP);
+ }
+ } else if (m_controlMode == kVoltage) {
+ if (getMessage(LM_API_VCOMP_COMP_RAMP, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ double rate = unpackFXP8_8(dataBuffer);
+
+ if (FXP8_EQ(rate, m_voltageRampRate))
+ m_voltageRampRateVerified = true;
+ else {
+ // It's wrong - set it again
+ SetVoltageRampRate(m_voltageRampRate);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_VCOMP_COMP_RAMP);
+ }
+ }
+ }
+
+ if (!m_faultTimeVerified) {
+ if (getMessage(LM_API_CFG_FAULT_TIME, CAN_MSGID_FULL_M, dataBuffer,
+ &dataSize)) {
+ uint16_t faultTime = unpackint16_t(dataBuffer);
+
+ if ((uint16_t)(m_faultTime * 1000.0) == faultTime)
+ m_faultTimeVerified = true;
+ else {
+ // It's wrong - set it again
+ ConfigFaultTime(m_faultTime);
+ }
+ } else {
+ // Verification is needed but not available - request it again.
+ requestMessage(LM_API_CFG_FAULT_TIME);
+ }
+ }
+
+ if (!m_receivedStatusMessage0 || !m_receivedStatusMessage1 ||
+ !m_receivedStatusMessage2) {
+ // If the periodic status messages haven't been verified as received,
+ // request periodic status messages again and attempt to unpack any
+ // available ones.
+ setupPeriodicStatus();
+ GetTemperature();
+ GetPosition();
+ GetFaults();
+ }
+}
+
+/**
+ * Set the reference source device for speed controller mode.
+ *
+ * Choose encoder as the source of speed feedback when in speed control mode.
+ *
+ * @param reference Specify a speed reference.
+ */
+void CANJaguar::SetSpeedReference(uint8_t reference) {
+ uint8_t dataBuffer[8];
+
+ // Send the speed reference parameter
+ dataBuffer[0] = reference;
+ sendMessage(LM_API_SPD_REF, dataBuffer, sizeof(uint8_t));
+
+ m_speedReference = reference;
+ m_speedRefVerified = false;
+}
+
+/**
+ * Get the reference source device for speed controller mode.
+ *
+ * @return A speed reference indicating the currently selected reference device
+ * for speed controller mode.
+ */
+uint8_t CANJaguar::GetSpeedReference() const { return m_speedReference; }
+
+/**
+ * Set the reference source device for position controller mode.
+ *
+ * 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.
+ */
+void CANJaguar::SetPositionReference(uint8_t reference) {
+ uint8_t dataBuffer[8];
+
+ // Send the position reference parameter
+ dataBuffer[0] = reference;
+ sendMessage(LM_API_POS_REF, dataBuffer, sizeof(uint8_t));
+
+ m_positionReference = reference;
+ m_posRefVerified = false;
+}
+
+/**
+ * Get the reference source device for position controller mode.
+ *
+ * @return A PositionReference indicating the currently selected reference
+ * device for position controller mode.
+ */
+uint8_t CANJaguar::GetPositionReference() const { return m_positionReference; }
+
+/**
+ * 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) {
+ SetP(p);
+ SetI(i);
+ SetD(d);
+}
+
+/**
+ * Set the P constant for the closed loop modes.
+ *
+ * @param p The proportional gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetP(double p) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ case kVoltage:
+ case kFollower:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ break;
+ case kSpeed:
+ dataSize = packFXP16_16(dataBuffer, p);
+ sendMessage(LM_API_SPD_PC, dataBuffer, dataSize);
+ break;
+ case kPosition:
+ dataSize = packFXP16_16(dataBuffer, p);
+ sendMessage(LM_API_POS_PC, dataBuffer, dataSize);
+ break;
+ case kCurrent:
+ dataSize = packFXP16_16(dataBuffer, p);
+ sendMessage(LM_API_ICTRL_PC, dataBuffer, dataSize);
+ break;
+ }
+
+ m_p = p;
+ m_pVerified = false;
+}
+
+/**
+ * Set the I constant for the closed loop modes.
+ *
+ * @param i The integral gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetI(double i) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ case kVoltage:
+ case kFollower:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ break;
+ case kSpeed:
+ dataSize = packFXP16_16(dataBuffer, i);
+ sendMessage(LM_API_SPD_IC, dataBuffer, dataSize);
+ break;
+ case kPosition:
+ dataSize = packFXP16_16(dataBuffer, i);
+ sendMessage(LM_API_POS_IC, dataBuffer, dataSize);
+ break;
+ case kCurrent:
+ dataSize = packFXP16_16(dataBuffer, i);
+ sendMessage(LM_API_ICTRL_IC, dataBuffer, dataSize);
+ break;
+ }
+
+ m_i = i;
+ m_iVerified = false;
+}
+
+/**
+ * Set the D constant for the closed loop modes.
+ *
+ * @param d The derivative gain of the Jaguar's PID controller.
+ */
+void CANJaguar::SetD(double d) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ case kVoltage:
+ case kFollower:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ break;
+ case kSpeed:
+ dataSize = packFXP16_16(dataBuffer, d);
+ sendMessage(LM_API_SPD_DC, dataBuffer, dataSize);
+ break;
+ case kPosition:
+ dataSize = packFXP16_16(dataBuffer, d);
+ sendMessage(LM_API_POS_DC, dataBuffer, dataSize);
+ break;
+ case kCurrent:
+ dataSize = packFXP16_16(dataBuffer, d);
+ sendMessage(LM_API_ICTRL_DC, dataBuffer, dataSize);
+ break;
+ }
+
+ m_d = d;
+ m_dVerified = false;
+}
+
+/**
+ * Get the Proportional gain of the controller.
+ *
+ * @return The proportional gain.
+ */
+double CANJaguar::GetP() const {
+ if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return 0.0;
+ }
+
+ return m_p;
+}
+
+/**
+ * Get the Intregral gain of the controller.
+ *
+ * @return The integral gain.
+ */
+double CANJaguar::GetI() const {
+ if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return 0.0;
+ }
+
+ return m_i;
+}
+
+/**
+ * Get the Differential gain of the controller.
+ *
+ * @return The differential gain.
+ */
+double CANJaguar::GetD() const {
+ if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) {
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "PID constants only apply in Speed, Position, and Current mode");
+ return 0.0;
+ }
+
+ return m_d;
+}
+
+/**
+ * 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:
+ sendMessage(LM_API_VOLT_T_EN, dataBuffer, dataSize);
+ break;
+ case kSpeed:
+ sendMessage(LM_API_SPD_T_EN, dataBuffer, dataSize);
+ break;
+ case kPosition:
+ dataSize = packFXP16_16(dataBuffer, encoderInitialPosition);
+ sendMessage(LM_API_POS_T_EN, dataBuffer, dataSize);
+ break;
+ case kCurrent:
+ sendMessage(LM_API_ICTRL_T_EN, dataBuffer, dataSize);
+ break;
+ case kVoltage:
+ sendMessage(LM_API_VCOMP_T_EN, dataBuffer, dataSize);
+ break;
+ default:
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "The Jaguar only supports Current, Voltage, "
+ "Position, Speed, and Percent (Throttle) "
+ "modes.");
+ return;
+ }
+
+ m_controlEnabled = true;
+ m_controlModeVerified = false;
+}
+
+/**
+ * Disable the closed loop controller.
+ *
+ * Stop driving the output based on the feedback.
+ */
+void CANJaguar::DisableControl() {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize = 0;
+
+ // Disable all control
+ sendMessage(LM_API_VOLT_DIS, dataBuffer, dataSize);
+ sendMessage(LM_API_SPD_DIS, dataBuffer, dataSize);
+ sendMessage(LM_API_POS_DIS, dataBuffer, dataSize);
+ sendMessage(LM_API_ICTRL_DIS, dataBuffer, dataSize);
+ sendMessage(LM_API_VCOMP_DIS, dataBuffer, dataSize);
+
+ // Stop all periodic setpoints
+ sendMessage(LM_API_VOLT_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+ sendMessage(LM_API_SPD_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+ sendMessage(LM_API_POS_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+ sendMessage(LM_API_ICTRL_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+ sendMessage(LM_API_VCOMP_T_SET, dataBuffer, dataSize,
+ CAN_SEND_PERIOD_STOP_REPEATING);
+
+ m_controlEnabled = false;
+}
+
+/**
+ * Enable controlling the motor voltage as a percentage of the bus voltage
+ * without any position or speed feedback.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ */
+void CANJaguar::SetPercentMode() {
+ SetControlMode(kPercentVbus);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_NONE);
+}
+
+/**
+ * Enable controlling the motor voltage as a percentage of the bus voltage,
+ * and enable speed sensing from a non-quadrature encoder.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param tag The constant CANJaguar::Encoder
+ * @param codesPerRev The counts per revolution on the encoder
+ */
+void CANJaguar::SetPercentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) {
+ SetControlMode(kPercentVbus);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+}
+
+/**
+ * Enable controlling the motor voltage as a percentage of the bus voltage,
+ * and enable speed sensing from a non-quadrature encoder.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param tag The constant CANJaguar::QuadEncoder
+ * @param codesPerRev The counts per revolution on the encoder
+ */
+void CANJaguar::SetPercentMode(CANJaguar::QuadEncoderStruct,
+ uint16_t codesPerRev) {
+ SetControlMode(kPercentVbus);
+ SetPositionReference(LM_REF_ENCODER);
+ SetSpeedReference(LM_REF_QUAD_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+}
+
+/**
+* Enable controlling the motor voltage as a percentage of the bus voltage,
+* and enable position sensing from a potentiometer and no speed feedback.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param potentiometer The constant CANJaguar::Potentiometer
+*/
+void CANJaguar::SetPercentMode(CANJaguar::PotentiometerStruct) {
+ SetControlMode(kPercentVbus);
+ SetPositionReference(LM_REF_POT);
+ SetSpeedReference(LM_REF_NONE);
+ ConfigPotentiometerTurns(1);
+}
+
+/**
+ * Enable controlling the motor current with a PID loop.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @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::SetCurrentMode(double p, double i, double d) {
+ SetControlMode(kCurrent);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_NONE);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the motor current with a PID loop, and enable speed
+* sensing from a non-quadrature encoder.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param encoder The constant CANJaguar::Encoder
+* @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::SetCurrentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev,
+ double p, double i, double d) {
+ SetControlMode(kCurrent);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_NONE);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the motor current with a PID loop, and enable speed and
+* position sensing from a quadrature encoder.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param endoer The constant CANJaguar::QuadEncoder
+* @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::SetCurrentMode(CANJaguar::QuadEncoderStruct,
+ uint16_t codesPerRev, double p, double i,
+ double d) {
+ SetControlMode(kCurrent);
+ SetPositionReference(LM_REF_ENCODER);
+ SetSpeedReference(LM_REF_QUAD_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the motor current with a PID loop, and enable position
+* sensing from a potentiometer.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param potentiometer The constant CANJaguar::Potentiometer
+* @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::SetCurrentMode(CANJaguar::PotentiometerStruct, double p,
+ double i, double d) {
+ SetControlMode(kCurrent);
+ SetPositionReference(LM_REF_POT);
+ SetSpeedReference(LM_REF_NONE);
+ ConfigPotentiometerTurns(1);
+ SetPID(p, i, d);
+}
+
+/**
+ * Enable controlling the speed with a feedback loop from a non-quadrature
+ * encoder.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param encoder The constant CANJaguar::Encoder
+ * @param codesPerRev The counts per revolution on the encoder.
+ * @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::SetSpeedMode(CANJaguar::EncoderStruct, uint16_t codesPerRev,
+ double p, double i, double d) {
+ SetControlMode(kSpeed);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the speed with a feedback loop from a quadrature
+* encoder.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param encoder The constant CANJaguar::QuadEncoder
+* @param codesPerRev The counts per revolution on the encoder.
+* @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::SetSpeedMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev,
+ double p, double i, double d) {
+ SetControlMode(kSpeed);
+ SetPositionReference(LM_REF_ENCODER);
+ SetSpeedReference(LM_REF_QUAD_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+ * Enable controlling the position with a feedback loop using an encoder.<br>
+ * After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+ * CANJaguar#EnableControl(double)} to enable the device.
+ *
+ * @param encoder The constant CANJaguar::QuadEncoder
+ * @param codesPerRev The counts per revolution on the encoder.
+ * @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::SetPositionMode(CANJaguar::QuadEncoderStruct,
+ uint16_t codesPerRev, double p, double i,
+ double d) {
+ SetControlMode(kPosition);
+ SetPositionReference(LM_REF_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+ SetPID(p, i, d);
+}
+
+/**
+* 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 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::SetPositionMode(CANJaguar::PotentiometerStruct, double p,
+ double i, double d) {
+ SetControlMode(kPosition);
+ SetPositionReference(LM_REF_POT);
+ ConfigPotentiometerTurns(1);
+ SetPID(p, i, d);
+}
+
+/**
+* Enable controlling the motor voltage without any position or speed
+* feedback.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*/
+void CANJaguar::SetVoltageMode() {
+ SetControlMode(kVoltage);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_NONE);
+}
+
+/**
+* Enable controlling the motor voltage with speed feedback from a
+* non-quadrature encoder and no position feedback.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param encoder The constant CANJaguar::Encoder
+* @param codesPerRev The counts per revolution on the encoder
+*/
+void CANJaguar::SetVoltageMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) {
+ SetControlMode(kVoltage);
+ SetPositionReference(LM_REF_NONE);
+ SetSpeedReference(LM_REF_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+}
+
+/**
+* Enable controlling the motor voltage with position and speed feedback from a
+* quadrature encoder.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param encoder The constant CANJaguar::QuadEncoder
+* @param codesPerRev The counts per revolution on the encoder
+*/
+void CANJaguar::SetVoltageMode(CANJaguar::QuadEncoderStruct,
+ uint16_t codesPerRev) {
+ SetControlMode(kVoltage);
+ SetPositionReference(LM_REF_ENCODER);
+ SetSpeedReference(LM_REF_QUAD_ENCODER);
+ ConfigEncoderCodesPerRev(codesPerRev);
+}
+
+/**
+* Enable controlling the motor voltage with position feedback from a
+* potentiometer and no speed feedback.<br>
+* After calling this you must call {@link CANJaguar#EnableControl()} or {@link
+* CANJaguar#EnableControl(double)} to enable the device.
+*
+* @param potentiometer The constant CANJaguar::Potentiometer
+*/
+void CANJaguar::SetVoltageMode(CANJaguar::PotentiometerStruct) {
+ SetControlMode(kVoltage);
+ SetPositionReference(LM_REF_POT);
+ SetSpeedReference(LM_REF_NONE);
+ ConfigPotentiometerTurns(1);
+}
+
+/**
+ * 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.
+ */
+void CANJaguar::SetControlMode(ControlMode controlMode) {
+ // Disable the previous mode
+ DisableControl();
+
+ if (controlMode == kFollower)
+ wpi_setWPIErrorWithContext(IncompatibleMode,
+ "The Jaguar only supports Current, Voltage, "
+ "Position, Speed, and Percent (Throttle) "
+ "modes.");
+
+ // Update the local mode
+ m_controlMode = controlMode;
+ m_controlModeVerified = false;
+
+ HALReport(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber,
+ m_controlMode);
+}
+
+/**
+ * Get the active control mode from the Jaguar.
+ *
+ * Ask the Jag what mode it is in.
+ *
+ * @return ControlMode that the Jag is in.
+ */
+CANJaguar::ControlMode CANJaguar::GetControlMode() const {
+ return m_controlMode;
+}
+
+/**
+ * Get the voltage at the battery input terminals of the Jaguar.
+ *
+ * @return The bus voltage in volts.
+ */
+float CANJaguar::GetBusVoltage() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_busVoltage;
+}
+
+/**
+ * Get the voltage being output from the motor terminals of the Jaguar.
+ *
+ * @return The output voltage in volts.
+ */
+float CANJaguar::GetOutputVoltage() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_outputVoltage;
+}
+
+/**
+ * Get the current through the motor terminals of the Jaguar.
+ *
+ * @return The output current in amps.
+ */
+float CANJaguar::GetOutputCurrent() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_outputCurrent;
+}
+
+/**
+ * Get the internal temperature of the Jaguar.
+ *
+ * @return The temperature of the Jaguar in degrees Celsius.
+ */
+float CANJaguar::GetTemperature() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_temperature;
+}
+
+/**
+ * Get the position of the encoder or potentiometer.
+ *
+ * @return The position of the motor in rotations based on the configured
+ * feedback.
+ * @see CANJaguar#ConfigPotentiometerTurns(int)
+ * @see CANJaguar#ConfigEncoderCodesPerRev(int)
+ */
+double CANJaguar::GetPosition() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_position;
+}
+
+/**
+ * Get the speed of the encoder.
+ *
+ * @return The speed of the motor in RPM based on the configured feedback.
+ */
+double CANJaguar::GetSpeed() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_speed;
+}
+
+/**
+ * Get the status of the forward limit switch.
+ *
+ * @return The motor is allowed to turn in the forward direction when true.
+ */
+bool CANJaguar::GetForwardLimitOK() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_limits & kForwardLimit;
+}
+
+/**
+ * Get the status of the reverse limit switch.
+ *
+ * @return The motor is allowed to turn in the reverse direction when true.
+ */
+bool CANJaguar::GetReverseLimitOK() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_limits & kReverseLimit;
+}
+
+/**
+ * Get the status of any faults the Jaguar has detected.
+ *
+ * @return A bit-mask of faults defined by the "Faults" enum.
+ * @see #kCurrentFault
+ * @see #kBusVoltageFault
+ * @see #kTemperatureFault
+ * @see #kGateDriverFault
+ */
+uint16_t CANJaguar::GetFaults() const {
+ updatePeriodicStatus();
+ std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+ return m_faults;
+}
+
+/**
+ * Set the maximum voltage change rate.
+ *
+ * When in PercentVbus or 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.
+ */
+void CANJaguar::SetVoltageRampRate(double rampRate) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+ uint32_t message;
+
+ switch (m_controlMode) {
+ case kPercentVbus:
+ dataSize = packPercentage(
+ dataBuffer, rampRate / (m_maxOutputVoltage * kControllerRate));
+ message = LM_API_VOLT_SET_RAMP;
+ break;
+ case kVoltage:
+ dataSize = packFXP8_8(dataBuffer, rampRate / kControllerRate);
+ message = LM_API_VCOMP_COMP_RAMP;
+ break;
+ default:
+ wpi_setWPIErrorWithContext(
+ IncompatibleMode,
+ "SetVoltageRampRate only applies in Voltage and Percent mode");
+ return;
+ }
+
+ sendMessage(message, dataBuffer, dataSize);
+
+ m_voltageRampRate = rampRate;
+ m_voltageRampRateVerified = false;
+}
+
+/**
+ * Get the version of the firmware running on the Jaguar.
+ *
+ * @return The firmware version. 0 if the device did not respond.
+ */
+uint32_t CANJaguar::GetFirmwareVersion() const { return m_firmwareVersion; }
+
+/**
+ * Get the version of the Jaguar hardware.
+ *
+ * @return The hardware version. 1: Jaguar, 2: Black Jaguar
+ */
+uint8_t CANJaguar::GetHardwareVersion() const { return m_hardwareVersion; }
+
+/**
+ * Configure what the controller does to the H-Bridge when neutral (not driving
+ * the output).
+ *
+ * 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.
+ */
+void CANJaguar::ConfigNeutralMode(NeutralMode mode) {
+ uint8_t dataBuffer[8];
+
+ // Set the neutral mode
+ sendMessage(LM_API_CFG_BRAKE_COAST, dataBuffer, sizeof(uint8_t));
+
+ m_neutralMode = mode;
+ m_neutralModeVerified = false;
+}
+
+/**
+ * Configure how many codes per revolution are generated by your encoder.
+ *
+ * @param codesPerRev The number of counts per revolution in 1X mode.
+ */
+void CANJaguar::ConfigEncoderCodesPerRev(uint16_t codesPerRev) {
+ uint8_t dataBuffer[8];
+
+ // Set the codes per revolution mode
+ packint16_t(dataBuffer, codesPerRev);
+ sendMessage(LM_API_CFG_ENC_LINES, dataBuffer, sizeof(uint16_t));
+
+ m_encoderCodesPerRev = codesPerRev;
+ m_encoderCodesPerRevVerified = false;
+}
+
+/**
+ * Configure the number of turns on the potentiometer.
+ *
+ * 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.
+ */
+void CANJaguar::ConfigPotentiometerTurns(uint16_t turns) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ // Set the pot turns
+ dataSize = packint16_t(dataBuffer, turns);
+ sendMessage(LM_API_CFG_POT_TURNS, dataBuffer, dataSize);
+
+ m_potentiometerTurns = turns;
+ m_potentiometerTurnsVerified = false;
+}
+
+/**
+ * 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) {
+ ConfigLimitMode(kLimitMode_SoftPositionLimits);
+ ConfigForwardLimit(forwardLimitPosition);
+ ConfigReverseLimit(reverseLimitPosition);
+}
+
+/**
+ * Disable Soft Position Limits if previously enabled.
+ *
+ * Soft Position Limits are disabled by default.
+ */
+void CANJaguar::DisableSoftPositionLimits() {
+ ConfigLimitMode(kLimitMode_SwitchInputsOnly);
+}
+
+/**
+ * Set the limit mode for position control mode.
+ *
+ * Use ConfigSoftPositionLimits or DisableSoftPositionLimits to set this
+ * automatically.
+ */
+void CANJaguar::ConfigLimitMode(LimitMode mode) {
+ uint8_t dataBuffer[8];
+
+ dataBuffer[0] = mode;
+ sendMessage(LM_API_CFG_LIMIT_MODE, dataBuffer, sizeof(uint8_t));
+
+ m_limitMode = mode;
+ m_limitModeVerified = false;
+}
+
+/**
+* Set the position that if exceeded will disable the forward direction.
+*
+* Use ConfigSoftPositionLimits to set this and the limit mode automatically.
+*/
+void CANJaguar::ConfigForwardLimit(double forwardLimitPosition) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ dataSize = packFXP16_16(dataBuffer, forwardLimitPosition);
+ dataBuffer[dataSize++] = 1;
+ sendMessage(LM_API_CFG_LIMIT_FWD, dataBuffer, dataSize);
+
+ m_forwardLimit = forwardLimitPosition;
+ m_forwardLimitVerified = false;
+}
+
+/**
+* Set the position that if exceeded will disable the reverse direction.
+*
+* Use ConfigSoftPositionLimits to set this and the limit mode automatically.
+*/
+void CANJaguar::ConfigReverseLimit(double reverseLimitPosition) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ dataSize = packFXP16_16(dataBuffer, reverseLimitPosition);
+ dataBuffer[dataSize++] = 0;
+ sendMessage(LM_API_CFG_LIMIT_REV, dataBuffer, dataSize);
+
+ m_reverseLimit = reverseLimitPosition;
+ m_reverseLimitVerified = false;
+}
+
+/**
+ * Configure the maximum voltage that the Jaguar will ever output.
+ *
+ * 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.
+ */
+void CANJaguar::ConfigMaxOutputVoltage(double voltage) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ dataSize = packFXP8_8(dataBuffer, voltage);
+ sendMessage(LM_API_CFG_MAX_VOUT, dataBuffer, dataSize);
+
+ m_maxOutputVoltage = voltage;
+ m_maxOutputVoltageVerified = false;
+}
+
+/**
+ * Configure how long the Jaguar waits in the case of a fault before resuming
+ * operation.
+ *
+ * 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.
+ */
+void CANJaguar::ConfigFaultTime(float faultTime) {
+ uint8_t dataBuffer[8];
+ uint8_t dataSize;
+
+ if (faultTime < 0.5)
+ faultTime = 0.5;
+ else if (faultTime > 3.0)
+ faultTime = 3.0;
+
+ // Message takes ms
+ dataSize = packint16_t(dataBuffer, (int16_t)(faultTime * 1000.0));
+ sendMessage(LM_API_CFG_FAULT_TIME, dataBuffer, dataSize);
+
+ m_faultTime = faultTime;
+ m_faultTimeVerified = false;
+}
+
+/**
+ * Update all the motors that have pending sets in the syncGroup.
+ *
+ * @param syncGroup A bitmask of groups to generate synchronous output.
+ */
+void CANJaguar::UpdateSyncGroup(uint8_t syncGroup) {
+ sendMessageHelper(CAN_MSGID_API_SYNC, &syncGroup, sizeof(syncGroup),
+ CAN_SEND_PERIOD_NO_REPEAT);
+}
+
+void CANJaguar::SetExpiration(float timeout) {
+ if (m_safetyHelper) m_safetyHelper->SetExpiration(timeout);
+}
+
+float CANJaguar::GetExpiration() const {
+ if (!m_safetyHelper) return 0.0;
+ return m_safetyHelper->GetExpiration();
+}
+
+bool CANJaguar::IsAlive() const {
+ if (!m_safetyHelper) return false;
+ return m_safetyHelper->IsAlive();
+}
+
+bool CANJaguar::IsSafetyEnabled() const {
+ if (!m_safetyHelper) return false;
+ return m_safetyHelper->IsSafetyEnabled();
+}
+
+void CANJaguar::SetSafetyEnabled(bool enabled) {
+ if (m_safetyHelper) m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+void CANJaguar::GetDescription(std::ostringstream& desc) const {
+ desc << "CANJaguar ID " << m_deviceNumber;
+}
+
+uint8_t CANJaguar::GetDeviceID() const { return m_deviceNumber; }
+
+/**
+ * Common interface for stopping the motor
+ * Part of the MotorSafety interface
+ *
+ * @deprecated Call DisableControl instead.
+ */
+void CANJaguar::StopMotor() { DisableControl(); }
+
+/**
+* Common interface for inverting direction of a speed controller.
+* Only works in PercentVbus, speed, and Voltage modes.
+* @param isInverted The state of inversion, true is inverted
+*/
+void CANJaguar::SetInverted(bool isInverted) { m_isInverted = isInverted; }
+
+/**
+ * Common interface for the inverting direction of a speed controller.
+ *
+ * @return isInverted The state of inversion, true is inverted.
+ *
+ */
+bool CANJaguar::GetInverted() const { return m_isInverted; }
+
+void CANJaguar::ValueChanged(ITable* source, llvm::StringRef key,
+ std::shared_ptr<nt::Value> value, bool isNew) {
+ if(key == "Mode" && value->IsDouble()) SetControlMode(static_cast<CANSpeedController::ControlMode>(value->GetDouble()));
+ if(IsModePID(m_controlMode) && value->IsDouble()) {
+ if(key == "p") SetP(value->GetDouble());
+ if(key == "i") SetI(value->GetDouble());
+ if(key == "d") SetD(value->GetDouble());
+ }
+ if(key == "Enabled" && value->IsBoolean()) {
+ if (value->GetBoolean()) {
+ EnableControl();
+ } else {
+ DisableControl();
+ }
+ }
+ if(key == "Value" && value->IsDouble()) Set(value->GetDouble());
+}
+
+bool CANJaguar::IsModePID(CANSpeedController::ControlMode mode) const {
+ return mode == kCurrent || mode == kSpeed || mode == kPosition;
+}
+
+void CANJaguar::UpdateTable() {
+ if (m_table != nullptr) {
+ m_table->PutString("~TYPE~", "CANSpeedController");
+ m_table->PutString("Type", "CANJaguar");
+ m_table->PutString("Mode", GetModeName(m_controlMode));
+ if (IsModePID(m_controlMode)) {
+ m_table->PutNumber("p", GetP());
+ m_table->PutNumber("i", GetI());
+ m_table->PutNumber("d", GetD());
+ }
+ m_table->PutBoolean("Enabled", m_controlEnabled);
+ m_table->PutNumber("Value", Get());
+ }
+}
+
+void CANJaguar::StartLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->AddTableListener(this, true);
+ }
+}
+
+void CANJaguar::StopLiveWindowMode() {
+ if (m_table != nullptr) {
+ m_table->RemoveTableListener(this);
+ }
+}
+
+std::string CANJaguar::GetSmartDashboardType() const {
+ return "CANSpeedController";
+}
+
+void CANJaguar::InitTable(std::shared_ptr<ITable> subTable) {
+ m_table = subTable;
+ UpdateTable();
+}
+
+std::shared_ptr<ITable> CANJaguar::GetTable() const { return m_table; }