mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-30 02:31:44 +00:00
Merge "CANJaguar uses periodic status updates [artf2621]"
This commit is contained in:
Brad Miller (WPI)
@@ -147,6 +147,9 @@ protected:
|
||||
void requestMessage(uint32_t messageID, int32_t period = CAN_SEND_PERIOD_NO_REPEAT);
|
||||
bool getMessage(uint32_t messageID, uint32_t mask, uint8_t *data, uint8_t *dataSize);
|
||||
|
||||
void setupPeriodicStatus();
|
||||
void updatePeriodicStatus();
|
||||
|
||||
uint8_t m_deviceNumber;
|
||||
float m_value;
|
||||
|
||||
@@ -196,15 +199,10 @@ protected:
|
||||
uint32_t m_firmwareVersion;
|
||||
uint8_t m_hardwareVersion;
|
||||
|
||||
// Which status values have we received at least once?
|
||||
bool m_receivedBusVoltage;
|
||||
bool m_receivedOutputVoltage;
|
||||
bool m_receivedOutputCurrent;
|
||||
bool m_receivedTemperature;
|
||||
bool m_receivedPosition;
|
||||
bool m_receivedSpeed;
|
||||
bool m_receivedLimits;
|
||||
bool m_receivedFaults;
|
||||
// Which periodic status messages have we received at least once?
|
||||
bool m_receivedStatusMessage0;
|
||||
bool m_receivedStatusMessage1;
|
||||
bool m_receivedStatusMessage2;
|
||||
|
||||
void verify();
|
||||
|
||||
|
||||
@@ -119,64 +119,49 @@ void CANJaguar::InitCANJaguar()
|
||||
m_voltageRampRateVerified = true;
|
||||
m_faultTimeVerified = true;
|
||||
|
||||
m_receivedStatusMessage0 = false;
|
||||
m_receivedStatusMessage1 = false;
|
||||
m_receivedStatusMessage2 = false;
|
||||
|
||||
bool receivedFirmwareVersion = false;
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
// Request all status data periodically
|
||||
requestMessage(LM_API_STATUS_VOLTBUS, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_VOUT, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_CURRENT, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_TEMP, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_POS, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_SPD, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_LIMIT, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_FAULT, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_POWER, kSendMessagePeriod);
|
||||
|
||||
// Request firmware and hardware version only once
|
||||
requestMessage(CAN_IS_FRAME_REMOTE | CAN_MSGID_API_FIRMVER);
|
||||
requestMessage(LM_API_HWVER);
|
||||
|
||||
m_receivedBusVoltage = false;
|
||||
m_receivedOutputVoltage = false;
|
||||
m_receivedOutputCurrent = false;
|
||||
m_receivedTemperature = false;
|
||||
m_receivedPosition = false;
|
||||
m_receivedSpeed = false;
|
||||
m_receivedLimits = false;
|
||||
m_receivedFaults = false;
|
||||
|
||||
// Wait until we've gotten all of the status data at least once.
|
||||
for(int i = 0; i < kReceiveStatusAttempts; i++)
|
||||
{
|
||||
Wait(0.001);
|
||||
|
||||
GetBusVoltage();
|
||||
GetOutputVoltage();
|
||||
GetOutputCurrent();
|
||||
GetTemperature();
|
||||
GetPosition();
|
||||
GetSpeed();
|
||||
GetForwardLimitOK();
|
||||
GetFaults();
|
||||
setupPeriodicStatus();
|
||||
updatePeriodicStatus();
|
||||
|
||||
if(m_receivedBusVoltage &&
|
||||
m_receivedOutputVoltage &&
|
||||
m_receivedOutputCurrent &&
|
||||
m_receivedTemperature &&
|
||||
m_receivedPosition &&
|
||||
m_receivedSpeed &&
|
||||
m_receivedLimits &&
|
||||
m_receivedFaults)
|
||||
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(getMessage(CAN_MSGID_API_FIRMVER, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
m_firmwareVersion = unpackint32_t(dataBuffer);
|
||||
else
|
||||
wpi_setWPIErrorWithContext(JaguarMessageNotFound, "getMessage");
|
||||
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];
|
||||
@@ -508,6 +493,81 @@ bool CANJaguar::getMessage(uint32_t messageID, uint32_t messageMask, uint8_t *da
|
||||
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 / 10);
|
||||
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() {
|
||||
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_busVoltage = unpackFXP8_8(data);
|
||||
m_outputVoltage = unpackFXP8_8(data + 2);
|
||||
m_outputCurrent = unpackFXP8_8(data + 4);
|
||||
m_temperature = unpackFXP8_8(data + 6);
|
||||
|
||||
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_position = unpackFXP16_16(data);
|
||||
m_speed = unpackFXP16_16(data + 4);
|
||||
|
||||
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_limits = data[0];
|
||||
m_faults = data[1];
|
||||
|
||||
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
|
||||
@@ -551,15 +611,7 @@ void CANJaguar::verify()
|
||||
if(m_controlMode == kVoltage || m_controlMode == kPercentVbus)
|
||||
SetVoltageRampRate(m_voltageRampRate);
|
||||
|
||||
requestMessage(LM_API_STATUS_VOLTBUS, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_VOUT, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_CURRENT, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_TEMP, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_POS, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_SPD, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_LIMIT, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_FAULT, kSendMessagePeriod);
|
||||
requestMessage(LM_API_STATUS_POWER, kSendMessagePeriod);
|
||||
setupPeriodicStatus();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1461,14 +1513,7 @@ CANJaguar::ControlMode CANJaguar::GetControlMode()
|
||||
*/
|
||||
float CANJaguar::GetBusVoltage()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_VOLTBUS, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_busVoltage = unpackFXP8_8(dataBuffer);
|
||||
m_receivedBusVoltage = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_busVoltage;
|
||||
}
|
||||
@@ -1480,14 +1525,7 @@ float CANJaguar::GetBusVoltage()
|
||||
*/
|
||||
float CANJaguar::GetOutputVoltage()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_VOUT, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_outputVoltage = unpackFXP8_8(dataBuffer);
|
||||
m_receivedOutputVoltage = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_outputVoltage;
|
||||
}
|
||||
@@ -1499,14 +1537,7 @@ float CANJaguar::GetOutputVoltage()
|
||||
*/
|
||||
float CANJaguar::GetOutputCurrent()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_CURRENT, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_outputCurrent = unpackFXP8_8(dataBuffer);
|
||||
m_receivedOutputCurrent = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_outputCurrent;
|
||||
}
|
||||
@@ -1518,14 +1549,7 @@ float CANJaguar::GetOutputCurrent()
|
||||
*/
|
||||
float CANJaguar::GetTemperature()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_TEMP, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_temperature = unpackFXP8_8(dataBuffer);
|
||||
m_receivedTemperature = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_temperature;
|
||||
}
|
||||
@@ -1537,14 +1561,7 @@ float CANJaguar::GetTemperature()
|
||||
*/
|
||||
double CANJaguar::GetPosition()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_POS, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_position = unpackFXP16_16(dataBuffer);
|
||||
m_receivedPosition = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_position;
|
||||
}
|
||||
@@ -1556,14 +1573,7 @@ double CANJaguar::GetPosition()
|
||||
*/
|
||||
double CANJaguar::GetSpeed()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_SPD, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_speed = unpackFXP16_16(dataBuffer);
|
||||
m_receivedSpeed = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_speed;
|
||||
}
|
||||
@@ -1575,14 +1585,7 @@ double CANJaguar::GetSpeed()
|
||||
*/
|
||||
bool CANJaguar::GetForwardLimitOK()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_LIMIT, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_limits = dataBuffer[0];
|
||||
m_receivedLimits = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_limits & kForwardLimit;
|
||||
}
|
||||
@@ -1594,14 +1597,7 @@ bool CANJaguar::GetForwardLimitOK()
|
||||
*/
|
||||
bool CANJaguar::GetReverseLimitOK()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_LIMIT, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_limits = dataBuffer[0];
|
||||
m_receivedLimits = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_limits & kReverseLimit;
|
||||
}
|
||||
@@ -1613,14 +1609,7 @@ bool CANJaguar::GetReverseLimitOK()
|
||||
*/
|
||||
uint16_t CANJaguar::GetFaults()
|
||||
{
|
||||
uint8_t dataBuffer[8];
|
||||
uint8_t dataSize;
|
||||
|
||||
if(getMessage(LM_API_STATUS_FAULT, CAN_MSGID_FULL_M, dataBuffer, &dataSize))
|
||||
{
|
||||
m_faults = unpackint16_t(dataBuffer);
|
||||
m_receivedFaults = true;
|
||||
}
|
||||
updatePeriodicStatus();
|
||||
|
||||
return m_faults;
|
||||
}
|
||||
|
||||
@@ -9,46 +9,48 @@
|
||||
#include "gtest/gtest.h"
|
||||
#include "TestBench.h"
|
||||
|
||||
#include "NetworkCommunication/CANSessionMux.h"
|
||||
|
||||
static constexpr double kExpectedBusVoltage = 14.0;
|
||||
static constexpr double kExpectedTemperature = 25.0;
|
||||
|
||||
static constexpr double kMotorTime = 0.5;
|
||||
|
||||
static constexpr double kEncoderSettlingTime = 0.25;
|
||||
static constexpr double kEncoderPositionTolerance = 5.0/360.0; // +/-5 degrees
|
||||
static constexpr double kEncoderSettlingTime = 1.0;
|
||||
static constexpr double kEncoderPositionTolerance = 10.0/360.0; // +/-10 degrees
|
||||
|
||||
static constexpr double kPotentiometerSettlingTime = 0.05;
|
||||
static constexpr double kPotentiometerSettlingTime = 1.0;
|
||||
static constexpr double kPotentiometerPositionTolerance = 10.0/360.0; // +/-10 degrees
|
||||
|
||||
// TODO test coverage for CANJaguar
|
||||
class CANJaguarTest : public testing::Test {
|
||||
protected:
|
||||
CANJaguar *m_jaguar;
|
||||
DigitalOutput *m_fakeForwardLimit, *m_fakeReverseLimit;
|
||||
AnalogOutput *m_fakePotentiometer;
|
||||
CANJaguar *m_jaguar;
|
||||
DigitalOutput *m_fakeForwardLimit, *m_fakeReverseLimit;
|
||||
AnalogOutput *m_fakePotentiometer;
|
||||
|
||||
virtual void SetUp() {
|
||||
m_jaguar = new CANJaguar(TestBench::kCANJaguarID);
|
||||
virtual void SetUp() {
|
||||
m_jaguar = new CANJaguar(TestBench::kCANJaguarID);
|
||||
|
||||
m_fakeForwardLimit = new DigitalOutput(TestBench::kFakeJaguarForwardLimit);
|
||||
m_fakeForwardLimit->Set(0);
|
||||
m_fakeForwardLimit = new DigitalOutput(TestBench::kFakeJaguarForwardLimit);
|
||||
m_fakeForwardLimit->Set(0);
|
||||
|
||||
m_fakeReverseLimit = new DigitalOutput(TestBench::kFakeJaguarReverseLimit);
|
||||
m_fakeReverseLimit->Set(0);
|
||||
m_fakeReverseLimit = new DigitalOutput(TestBench::kFakeJaguarReverseLimit);
|
||||
m_fakeReverseLimit->Set(0);
|
||||
|
||||
m_fakePotentiometer = new AnalogOutput(TestBench::kFakeJaguarPotentiometer);
|
||||
m_fakePotentiometer->SetVoltage(0.0f);
|
||||
m_fakePotentiometer = new AnalogOutput(TestBench::kFakeJaguarPotentiometer);
|
||||
m_fakePotentiometer->SetVoltage(0.0f);
|
||||
|
||||
/* The motor might still have momentum from the previous test. */
|
||||
//Wait(kEncoderSettlingTime);
|
||||
}
|
||||
/* The motor might still have momentum from the previous test. */
|
||||
Wait(kEncoderSettlingTime);
|
||||
}
|
||||
|
||||
virtual void TearDown() {
|
||||
delete m_jaguar;
|
||||
delete m_fakeForwardLimit;
|
||||
delete m_fakeReverseLimit;
|
||||
delete m_fakePotentiometer;
|
||||
}
|
||||
virtual void TearDown() {
|
||||
delete m_jaguar;
|
||||
delete m_fakeForwardLimit;
|
||||
delete m_fakeReverseLimit;
|
||||
delete m_fakePotentiometer;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -56,45 +58,45 @@ protected:
|
||||
* really getting status data from the Jaguar.
|
||||
*/
|
||||
TEST_F(CANJaguarTest, InitialStatus) {
|
||||
m_jaguar->SetPercentMode();
|
||||
m_jaguar->SetPercentMode();
|
||||
|
||||
EXPECT_NEAR(m_jaguar->GetBusVoltage(), kExpectedBusVoltage, 3.0)
|
||||
<< "Bus voltage is not a plausible value.";
|
||||
EXPECT_NEAR(m_jaguar->GetBusVoltage(), kExpectedBusVoltage, 3.0)
|
||||
<< "Bus voltage is not a plausible value.";
|
||||
|
||||
EXPECT_FLOAT_EQ(m_jaguar->GetOutputVoltage(), 0.0)
|
||||
<< "Output voltage is non-zero.";
|
||||
EXPECT_FLOAT_EQ(m_jaguar->GetOutputVoltage(), 0.0)
|
||||
<< "Output voltage is non-zero.";
|
||||
|
||||
EXPECT_FLOAT_EQ(m_jaguar->GetOutputCurrent(), 0.0)
|
||||
<< "Output current is non-zero.";
|
||||
EXPECT_FLOAT_EQ(m_jaguar->GetOutputCurrent(), 0.0)
|
||||
<< "Output current is non-zero.";
|
||||
|
||||
EXPECT_NEAR(m_jaguar->GetTemperature(), kExpectedTemperature, 5.0)
|
||||
<< "Temperature is not a plausible value.";
|
||||
EXPECT_NEAR(m_jaguar->GetTemperature(), kExpectedTemperature, 5.0)
|
||||
<< "Temperature is not a plausible value.";
|
||||
|
||||
EXPECT_EQ(m_jaguar->GetFaults(), 0)
|
||||
<< "Jaguar has one or more fault set.";
|
||||
EXPECT_EQ(m_jaguar->GetFaults(), 0)
|
||||
<< "Jaguar has one or more fault set.";
|
||||
}
|
||||
|
||||
/**
|
||||
* Test if we can drive the motor in percentage mode and get a position back
|
||||
*/
|
||||
TEST_F(CANJaguarTest, PercentForwards) {
|
||||
m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->Set(0.0f);
|
||||
m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
/* The motor might still have momentum from the previous test. */
|
||||
Wait(kEncoderSettlingTime);
|
||||
/* The motor might still have momentum from the previous test. */
|
||||
Wait(kEncoderSettlingTime);
|
||||
|
||||
double initialPosition = m_jaguar->GetPosition();
|
||||
double initialPosition = m_jaguar->GetPosition();
|
||||
|
||||
/* Drive the speed controller briefly to move the encoder */
|
||||
m_jaguar->Set(1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
/* Drive the speed controller briefly to move the encoder */
|
||||
m_jaguar->Set(1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
/* The position should have increased */
|
||||
EXPECT_GT(m_jaguar->GetPosition(), initialPosition)
|
||||
<< "CAN Jaguar position should have increased after the motor moved";
|
||||
/* The position should have increased */
|
||||
EXPECT_GT(m_jaguar->GetPosition(), initialPosition)
|
||||
<< "CAN Jaguar position should have increased after the motor moved";
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -102,23 +104,23 @@ TEST_F(CANJaguarTest, PercentForwards) {
|
||||
* position back
|
||||
*/
|
||||
TEST_F(CANJaguarTest, PercentReverse) {
|
||||
m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->Set(0.0f);
|
||||
m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
/* The motor might still have momentum from the previous test. */
|
||||
Wait(kEncoderSettlingTime);
|
||||
/* The motor might still have momentum from the previous test. */
|
||||
Wait(kEncoderSettlingTime);
|
||||
|
||||
double initialPosition = m_jaguar->GetPosition();
|
||||
double initialPosition = m_jaguar->GetPosition();
|
||||
|
||||
/* Drive the speed controller briefly to move the encoder */
|
||||
m_jaguar->Set(-1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
/* Drive the speed controller briefly to move the encoder */
|
||||
m_jaguar->Set(-1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
/* The position should have decreased */
|
||||
EXPECT_LT(m_jaguar->GetPosition(), initialPosition)
|
||||
<< "CAN Jaguar position should have decreased after the motor moved";
|
||||
/* The position should have decreased */
|
||||
EXPECT_LT(m_jaguar->GetPosition(), initialPosition)
|
||||
<< "CAN Jaguar position should have decreased after the motor moved";
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -126,17 +128,24 @@ TEST_F(CANJaguarTest, PercentReverse) {
|
||||
* the Jaguar.
|
||||
*/
|
||||
TEST_F(CANJaguarTest, EncoderPositionPID) {
|
||||
m_jaguar->SetPositionMode(CANJaguar::QuadEncoder, 360, 5.0f, 0.1f, 2.0f);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->SetPositionMode(CANJaguar::QuadEncoder, 360, 10.0f, 0.4f, 0.2f);
|
||||
m_jaguar->EnableControl();
|
||||
|
||||
double setpoint = m_jaguar->GetPosition() + 10.0f;
|
||||
double setpoint = m_jaguar->GetPosition() + 10.0f;
|
||||
|
||||
/* It should get to the setpoint within 10 seconds */
|
||||
m_jaguar->Set(setpoint);
|
||||
Wait(10.0f);
|
||||
/* It should get to the setpoint within 10 seconds */
|
||||
for(int i = 0; i < 10; i++) {
|
||||
m_jaguar->Set(setpoint);
|
||||
|
||||
EXPECT_NEAR(setpoint, m_jaguar->GetPosition(), kEncoderPositionTolerance)
|
||||
<< "CAN Jaguar should have reached setpoint with PID control";
|
||||
Wait(1.0f);
|
||||
|
||||
if(std::abs(m_jaguar->GetPosition() - setpoint) <= kEncoderPositionTolerance) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
EXPECT_NEAR(setpoint, m_jaguar->GetPosition(), kEncoderPositionTolerance)
|
||||
<< "CAN Jaguar should have reached setpoint with PID control";
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -144,28 +153,29 @@ TEST_F(CANJaguarTest, EncoderPositionPID) {
|
||||
* as a fake potentiometer.
|
||||
*/
|
||||
TEST_F(CANJaguarTest, FakePotentiometerPosition) {
|
||||
m_jaguar->SetPercentMode(CANJaguar::Potentiometer);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->SetPercentMode(CANJaguar::Potentiometer);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
m_fakePotentiometer->SetVoltage(0.0f);
|
||||
Wait(kPotentiometerSettlingTime);
|
||||
EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
|
||||
<< "CAN Jaguar should have returned the potentiometer position set by the analog output";
|
||||
m_fakePotentiometer->SetVoltage(0.0f);
|
||||
Wait(kPotentiometerSettlingTime);
|
||||
EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
|
||||
<< "CAN Jaguar should have returned the potentiometer position set by the analog output";
|
||||
|
||||
m_fakePotentiometer->SetVoltage(1.0f);
|
||||
Wait(kPotentiometerSettlingTime);
|
||||
EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
|
||||
<< "CAN Jaguar should have returned the potentiometer position set by the analog output";
|
||||
m_fakePotentiometer->SetVoltage(1.0f);
|
||||
Wait(kPotentiometerSettlingTime);
|
||||
EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
|
||||
<< "CAN Jaguar should have returned the potentiometer position set by the analog output";
|
||||
|
||||
m_fakePotentiometer->SetVoltage(2.0f);
|
||||
Wait(kPotentiometerSettlingTime);
|
||||
EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
|
||||
<< "CAN Jaguar should have returned the potentiometer position set by the analog output";
|
||||
m_fakePotentiometer->SetVoltage(2.0f);
|
||||
Wait(kPotentiometerSettlingTime);
|
||||
EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
|
||||
<< "CAN Jaguar should have returned the potentiometer position set by the analog output";
|
||||
|
||||
m_fakePotentiometer->SetVoltage(3.0f);
|
||||
Wait(kPotentiometerSettlingTime);
|
||||
EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
|
||||
<< "CAN Jaguar should have returned the potentiometer position set by the analog output";
|
||||
m_fakePotentiometer->SetVoltage(3.0f);
|
||||
Wait(kPotentiometerSettlingTime);
|
||||
EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
|
||||
<< "CAN Jaguar should have returned the potentiometer position set by the analog output";
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -173,40 +183,40 @@ TEST_F(CANJaguarTest, FakePotentiometerPosition) {
|
||||
* limit switch.
|
||||
*/
|
||||
TEST_F(CANJaguarTest, FakeLimitSwitchForwards) {
|
||||
m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
|
||||
m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly);
|
||||
m_fakeForwardLimit->Set(1);
|
||||
m_fakeReverseLimit->Set(0);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
|
||||
m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly);
|
||||
m_fakeForwardLimit->Set(1);
|
||||
m_fakeReverseLimit->Set(0);
|
||||
m_jaguar->EnableControl();
|
||||
|
||||
m_jaguar->Set(0.0f);
|
||||
Wait(kEncoderSettlingTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
Wait(kEncoderSettlingTime);
|
||||
|
||||
/* Make sure we limits are recognized by the Jaguar. */
|
||||
ASSERT_FALSE(m_jaguar->GetForwardLimitOK());
|
||||
ASSERT_TRUE(m_jaguar->GetReverseLimitOK());
|
||||
/* Make sure the limits are recognized by the Jaguar. */
|
||||
ASSERT_FALSE(m_jaguar->GetForwardLimitOK());
|
||||
ASSERT_TRUE(m_jaguar->GetReverseLimitOK());
|
||||
|
||||
double initialPosition = m_jaguar->GetPosition();
|
||||
double initialPosition = m_jaguar->GetPosition();
|
||||
|
||||
/* Drive the speed controller briefly to move the encoder. If the limit
|
||||
switch is recognized, it shouldn't actually move. */
|
||||
m_jaguar->Set(1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
/* Drive the speed controller briefly to move the encoder. If the limit
|
||||
switch is recognized, it shouldn't actually move. */
|
||||
m_jaguar->Set(1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
/* The position should be the same, since the limit switch was on. */
|
||||
EXPECT_NEAR(initialPosition, m_jaguar->GetPosition(), kEncoderPositionTolerance)
|
||||
<< "CAN Jaguar should not have moved with the limit switch pressed";
|
||||
/* The position should be the same, since the limit switch was on. */
|
||||
EXPECT_NEAR(initialPosition, m_jaguar->GetPosition(), kEncoderPositionTolerance)
|
||||
<< "CAN Jaguar should not have moved with the limit switch pressed";
|
||||
|
||||
/* Drive the speed controller in the other direction. It should actually
|
||||
move, since only the forward switch is activated.*/
|
||||
m_jaguar->Set(-1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
/* Drive the speed controller in the other direction. It should actually
|
||||
move, since only the forward switch is activated.*/
|
||||
m_jaguar->Set(-1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
/* The position should have decreased */
|
||||
EXPECT_LT(m_jaguar->GetPosition(), initialPosition)
|
||||
<< "CAN Jaguar should have moved in reverse while the forward limit was on";
|
||||
/* The position should have decreased */
|
||||
EXPECT_LT(m_jaguar->GetPosition(), initialPosition)
|
||||
<< "CAN Jaguar should have moved in reverse while the forward limit was on";
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -214,40 +224,40 @@ TEST_F(CANJaguarTest, FakeLimitSwitchForwards) {
|
||||
* switch.
|
||||
*/
|
||||
TEST_F(CANJaguarTest, FakeLimitSwitchReverse) {
|
||||
m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
|
||||
m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly);
|
||||
m_fakeForwardLimit->Set(0);
|
||||
m_fakeReverseLimit->Set(1);
|
||||
m_jaguar->EnableControl();
|
||||
m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
|
||||
m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly);
|
||||
m_fakeForwardLimit->Set(0);
|
||||
m_fakeReverseLimit->Set(1);
|
||||
m_jaguar->EnableControl();
|
||||
|
||||
m_jaguar->Set(0.0f);
|
||||
Wait(kEncoderSettlingTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
Wait(kEncoderSettlingTime);
|
||||
|
||||
/* Make sure we limits are recognized by the Jaguar. */
|
||||
ASSERT_TRUE(m_jaguar->GetForwardLimitOK());
|
||||
ASSERT_FALSE(m_jaguar->GetReverseLimitOK());
|
||||
/* Make sure the limits are recognized by the Jaguar. */
|
||||
ASSERT_TRUE(m_jaguar->GetForwardLimitOK());
|
||||
ASSERT_FALSE(m_jaguar->GetReverseLimitOK());
|
||||
|
||||
double initialPosition = m_jaguar->GetPosition();
|
||||
double initialPosition = m_jaguar->GetPosition();
|
||||
|
||||
/* Drive the speed controller backwards briefly to move the encoder. If
|
||||
the limit switch is recognized, it shouldn't actually move. */
|
||||
m_jaguar->Set(-1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
/* Drive the speed controller backwards briefly to move the encoder. If
|
||||
the limit switch is recognized, it shouldn't actually move. */
|
||||
m_jaguar->Set(-1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
/* The position should be the same, since the limit switch was on. */
|
||||
EXPECT_NEAR(initialPosition, m_jaguar->GetPosition(), kEncoderPositionTolerance)
|
||||
<< "CAN Jaguar should not have moved with the limit switch pressed";
|
||||
/* The position should be the same, since the limit switch was on. */
|
||||
EXPECT_NEAR(initialPosition, m_jaguar->GetPosition(), kEncoderPositionTolerance)
|
||||
<< "CAN Jaguar should not have moved with the limit switch pressed";
|
||||
|
||||
Wait(kEncoderSettlingTime);
|
||||
Wait(kEncoderSettlingTime);
|
||||
|
||||
/* Drive the speed controller in the other direction. It should actually
|
||||
move, since only the reverse switch is activated.*/
|
||||
m_jaguar->Set(1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
/* Drive the speed controller in the other direction. It should actually
|
||||
move, since only the reverse switch is activated.*/
|
||||
m_jaguar->Set(1.0f);
|
||||
Wait(kMotorTime);
|
||||
m_jaguar->Set(0.0f);
|
||||
|
||||
/* The position should have increased */
|
||||
EXPECT_GT(m_jaguar->GetPosition(), initialPosition)
|
||||
<< "CAN Jaguar should have moved forwards while the reverse limit was on";
|
||||
/* The position should have increased */
|
||||
EXPECT_GT(m_jaguar->GetPosition(), initialPosition)
|
||||
<< "CAN Jaguar should have moved forwards while the reverse limit was on";
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user