Merge "Updated CAN Jaguar for C++"

wpilibc/wpilibC++/include/CANJaguar.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2009. All Rights Reserved.							  */
+/* 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.  */
 /*----------------------------------------------------------------------------*/
@@ -14,6 +14,7 @@
 #include "HAL/HAL.hpp"
 #include "LiveWindow/LiveWindowSendable.h"
 #include "tables/ITable.h"
+#include "NetworkCommunication/CANSessionMux.h"
 
 #include <utility>
 
@@ -55,6 +56,9 @@ public:
 	SpeedReference GetSpeedReference();
 	void SetPositionReference(PositionReference reference);
 	PositionReference GetPositionReference();
+	void SetP(double p);
+	void SetI(double i);
+	void SetD(double d);
 	void SetPID(double p, double i, double d);
 	double GetP();
 	double GetI();
@@ -80,6 +84,9 @@ public:
 	void ConfigPotentiometerTurns(uint16_t turns);
 	void ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition);
 	void DisableSoftPositionLimits();
+	void ConfigLimitMode(LimitMode mode);
+	void ConfigForwardLimit(double forwardLimitPosition);
+	void ConfigReverseLimit(double reverseLimitPosition);
 	void ConfigMaxOutputVoltage(double voltage);
 	void ConfigFaultTime(float faultTime);
 
@@ -104,52 +111,62 @@ protected:
 	double unpackFXP16_16(uint8_t *buffer);
 	int16_t unpackint16_t(uint8_t *buffer);
 	int32_t unpackint32_t(uint8_t *buffer);
-	virtual void setTransaction(uint32_t messageID, const uint8_t *data, uint8_t dataSize);
-	virtual bool getTransaction(uint32_t messageID, uint8_t *data, uint8_t *dataSize);
 
-	static int32_t sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize);
-	static int32_t receiveMessage(uint32_t *messageID, uint8_t *data, uint8_t *dataSize);
+
+	void sendMessage(uint32_t messageID, const uint8_t *data, uint8_t dataSize, int32_t period = CAN_SEND_PERIOD_NO_REPEAT);
+	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);
 
 	uint8_t m_deviceNumber;
+	float m_value;
+
+	// Parameters/configuration
 	ControlMode m_controlMode;
-	MUTEX_ID m_transactionSemaphore;
+	SpeedReference m_speedReference;
+	PositionReference m_positionReference;
+	double m_p;
+	double m_i;
+	double m_d;
+	NeutralMode m_neutralMode;
+	uint16_t m_encoderCodesPerRev;
+	uint16_t m_potentiometerTurns;
+	LimitMode m_limitMode;
+	double m_forwardLimit;
+	double m_reverseLimit;
 	double m_maxOutputVoltage;
+	double m_voltageRampRate;
+	float m_faultTime;
 
-    enum CANValue {
-        CAN_VALUE = 0x1,
-        CAN_SPEED_REFERENCE = 0x2,
-        CAN_POSITION_REFERENCE = 0x4,
-        CAN_P = 0x8, CAN_I = 0x10, CAN_D = 0x20,
-        CAN_BUS_VOLTAGE = 0x40,
-        CAN_OUTPUT_VOLTAGE = 0x80,
-        CAN_OUTPUT_CURRENT = 0x100,
-        CAN_TEMPERATURE = 0x200,
-        CAN_POSITION = 0x400,
-        CAN_SPEED = 0x800,
-        CAN_LIMITS = 0x1000,
-        CAN_FAULTS = 0x2000,
-        CAN_FIRMWARE_VERSION = 0x4000,
-        CAN_HARDWARE_VERSION = 0x8000,
-        CAN_EVERYTHING = 0xffff
-    };
+	// Which parameters have been verified since they were last set?
+	bool m_controlModeVerified;
+	bool m_speedRefVerified;
+	bool m_posRefVerified;
+	bool m_pVerified;
+	bool m_iVerified;
+	bool m_dVerified;
+	bool m_neutralModeVerified;
+	bool m_encoderCodesPerRevVerified;
+	bool m_potentiometerTurnsVerified;
+	bool m_forwardLimitVerified;
+	bool m_reverseLimitVerified;
+	bool m_limitModeVerified;
+	bool m_maxOutputVoltageVerified;
+	bool m_voltageRampRateVerified;
+	bool m_faultTimeVerified;
 
-    bool isUnverified(CANValue value) const;
-    void verifyCANValues();
+	// Status data
+	float m_busVoltage;
+	float m_outputVoltage;
+	float m_outputCurrent;
+	float m_temperature;
+	double m_position;
+	double m_speed;
+	uint8_t m_limits;
+	uint16_t m_faults;
+	uint32_t m_firmwareVersion;
+	uint8_t m_hardwareVersion;
 
-    // Keep track of what cached CAN values have been verified.
-    CANValue m_verified_values;
-
-    // Cached CAN data
-    float m_value;
-    SpeedReference m_speedReference;
-    PositionReference m_positionReference;
-    double m_p, m_i, m_d;
-    float m_busVoltage, m_outputVoltage, m_outputCurrent, m_temperature;
-    double m_position, m_speed;
-    uint8_t m_limits;
-    uint16_t m_faults;
-    uint32_t m_firmwareVersion;
-    uint8_t m_hardwareVersion;
+	void verify();
 
 	MotorSafetyHelper *m_safetyHelper;
 

wpilibc/wpilibC++/include/WPIErrors.h

@@ -45,6 +45,7 @@ S(LineNotOutput, -27, "Cannot SetDigitalOutput for a line not configured for out
 S(ParameterOutOfRange, -28, "A parameter is out of range.");
 S(SPIClockRateTooLow, -29, "SPI clock rate was below the minimum supported");
 S(JaguarVersionError, -30, "Jaguar firmware version error");
+S(JaguarMessageNotFound, -31, "Jaguar message not found");
 S(NetworkTablesReadError, -40, "Error reading NetworkTables socket");
 S(NetworkTablesBufferFull, -41, "Buffer full writing to NetworkTables socket");
 S(NetworkTablesWrongType, -42, "The wrong type was read from the NetworkTables entry");
@@ -72,4 +73,3 @@ S(SPIReadNoData, 14, "No data available to read from SPI");
 S(IncompatibleState, 15, "Incompatible State: The operation cannot be completed");
 
 #undef S
-

wpilibc/wpilibC++IntegrationTests/include/TestBench.h

@@ -18,6 +18,7 @@ public:
 
 	/* Analog output channels */
 	static const uint32_t kAnalogOutputChannel = 0;
+	static const uint32_t kFakeJaguarPotentiometer = 1;
 
 	/* DIO channels */
 	static const uint32_t kTalonEncoderChannelA = 0;
@@ -44,12 +45,14 @@ public:
 	static const uint32_t kFakeSolenoid2Channel = 13;
 	static const uint32_t kFakeRelayForward = 14;
 	static const uint32_t kFakeRelayReverse = 15;
+	static const uint32_t kFakeJaguarForwardLimit = 16;
+	static const uint32_t kFakeJaguarReverseLimit = 17;
 
 	/* Relay channels */
 	static const uint32_t kRelayChannel = 0;
 
 	/* CAN IDs */
-	static const uint32_t kCANJaguarID = 1;
+	static const uint32_t kCANJaguarID = 2;
 
 	/* PDP channels */
 	static const uint32_t kJaguarPDPChannel = 7;

wpilibc/wpilibC++IntegrationTests/src/CANJaguarTest.cpp

@@ -9,36 +9,188 @@
 #include "gtest/gtest.h"
 #include "TestBench.h"
 
+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 kPotentiometerSettlingTime = 0.05;
+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;
+
+    double m_initialPosition;
 
     virtual void SetUp() {
-        Wait(0.04);
         m_jaguar = new CANJaguar(TestBench::kCANJaguarID);
+        m_jaguar->ChangeControlMode(CANJaguar::kPercentVbus);
+        m_jaguar->SetPositionReference(CANJaguar::kPosRef_QuadEncoder);
+        m_jaguar->ConfigEncoderCodesPerRev(360);
+
+        m_fakeForwardLimit = new DigitalOutput(TestBench::kFakeJaguarForwardLimit);
+        m_fakeForwardLimit->Set(0);
+
+        m_fakeReverseLimit = new DigitalOutput(TestBench::kFakeJaguarReverseLimit);
+        m_fakeReverseLimit->Set(0);
+
+        m_fakePotentiometer = new AnalogOutput(TestBench::kFakeJaguarPotentiometer);
+        m_fakePotentiometer->SetVoltage(0.0f);
+
+        /* The motor might still have momentum from the previous test. */
+        Wait(kEncoderSettlingTime);
+        m_initialPosition = m_jaguar->GetPosition();
     }
 
     virtual void TearDown() {
         delete m_jaguar;
-    }
-
-    void Reset() {
-        m_jaguar->ChangeControlMode(CANJaguar::kPercentVbus);
-        m_jaguar->Set(0.0f);
+        delete m_fakeForwardLimit;
+        delete m_fakeReverseLimit;
+        delete m_fakePotentiometer;
     }
 };
 
-#if 0
-TEST_F(CANJaguarTest, Speed) {
-    Reset();
+/**
+ * Test if we can drive the motor in percentage mode and get a position back
+ */
+TEST_F(CANJaguarTest, PercentForwards) {
+    /* Drive the speed controller briefly to move the encoder */
+    m_jaguar->Set(1.0f);
+    Wait(kMotorTime);
+    m_jaguar->Set(0.0f);
 
-    for(;;) {
-        m_jaguar->Set(1.0f);
-
-        std::cout << "Temperature = " << m_jaguar->GetTemperature() << std::endl;
-        std::cout << "Current = " << m_jaguar->GetOutputCurrent() << std::endl;
-
-        Wait(0.1);
-    }
+    /* The position should have increased */
+    EXPECT_GT(m_jaguar->GetPosition(), m_initialPosition)
+        << "CAN Jaguar position should have increased after the motor moved";
+}
+
+/**
+ * Test if we can drive the motor backwards in percentage mode and get a
+ * position back
+ */
+TEST_F(CANJaguarTest, PercentReverse) {
+    /* 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(), m_initialPosition)
+        << "CAN Jaguar position should have decreased after the motor moved";
+}
+
+/**
+ * Test if we can set a position and reach that position with PID control on
+ * the Jaguar.
+ */
+TEST_F(CANJaguarTest, EncoderPositionPID) {
+    m_jaguar->ChangeControlMode(CANJaguar::kPosition);
+    m_jaguar->SetPID(5.0f, 0.1f, 2.0f);
+    m_jaguar->EnableControl();
+
+    double setpoint = m_initialPosition + 10.0f;
+
+    /* It should get to the setpoint within 5 seconds */
+    m_jaguar->Set(setpoint);
+    Wait(5.0f);
+
+    EXPECT_NEAR(setpoint, m_jaguar->GetPosition(), kEncoderPositionTolerance)
+        << "CAN Jaguar should have reached setpoint with PID control";
+}
+
+/**
+ * Test if we can get a position in potentiometer mode, using an analog output
+ * as a fake potentiometer.
+ */
+TEST_F(CANJaguarTest, FakePotentiometerPosition) {
+    m_jaguar->SetPositionReference(CANJaguar::kPosRef_Potentiometer);
+    m_jaguar->ConfigPotentiometerTurns(1);
+
+    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(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";
+}
+
+/**
+ * Test if we can limit the Jaguar to only moving in reverse with a fake
+ * limit switch.
+ */
+TEST_F(CANJaguarTest, FakeLimitSwitchForwards) {
+    m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly);
+    m_fakeForwardLimit->Set(1);
+    m_fakeReverseLimit->Set(0);
+
+    /* 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(m_initialPosition, m_jaguar->GetPosition(), kEncoderPositionTolerance)
+        << "CAN Jaguar should not have moved with the limit switch pressed";
+
+    Wait(kEncoderSettlingTime);
+
+    /* 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(), m_initialPosition)
+        << "CAN Jaguar should have moved in reverse while the forward limit was on";
+}
+
+/**
+ * Test if we can limit the Jaguar to only moving forwards with a fake limit
+ * switch.
+ */
+TEST_F(CANJaguarTest, FakeLimitSwitchReverse) {
+    m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly);
+    m_fakeForwardLimit->Set(0);
+    m_fakeReverseLimit->Set(1);
+
+    /* 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(m_initialPosition, m_jaguar->GetPosition(), kEncoderPositionTolerance)
+        << "CAN Jaguar should not have moved with the limit switch pressed";
+
+    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);
+
+    /* The position should have increased */
+    EXPECT_GT(m_jaguar->GetPosition(), m_initialPosition)
+        << "CAN Jaguar should have moved forwards while the reverse limit was on";
 }
-#endif