More C++ tests

Change-Id: I92ce014a8ebe1c3b97f27aa15476fc3101cc1f1c

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

@@ -0,0 +1,154 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "WPILib.h"
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+enum MotorEncoderTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
+
+std::ostream &operator<<(std::ostream &os, MotorEncoderTestType const &type) {
+	switch(type) {
+	case TEST_VICTOR: os << "Victor"; break;
+	case TEST_JAGUAR: os << "Jaguar"; break;
+	case TEST_TALON: os << "Talon"; break;
+	}
+	
+	return os;
+}
+
+static constexpr double kMotorTime = 0.5;
+
+/**
+ * A fixture that includes a PWM speed controller and an encoder connected to
+ * the same motor.
+ * @author Thomas Clark
+ */
+class MotorEncoderTest : public testing::TestWithParam<MotorEncoderTestType> {
+protected:
+	SpeedController *m_speedController;
+	Encoder *m_encoder;
+	
+	virtual void SetUp() {
+		switch(GetParam()) {
+		case TEST_VICTOR:
+			m_speedController = new Victor(TestBench::kVictorChannel);
+			m_encoder = new Encoder(TestBench::kVictorEncoderChannelA,
+				TestBench::kVictorEncoderChannelB);
+			break;
+		
+		case TEST_JAGUAR:
+			m_speedController = new Jaguar(TestBench::kJaguarChannel);
+			m_encoder = new Encoder(TestBench::kJaguarEncoderChannelA,
+				TestBench::kJaguarEncoderChannelB);
+			break;
+		
+		case TEST_TALON:
+			m_speedController = new Talon(TestBench::kTalonChannel);
+			m_encoder = new Encoder(TestBench::kTalonEncoderChannelA,
+				TestBench::kTalonEncoderChannelB);
+			break;
+		}
+		
+	}
+	
+	virtual void TearDown() {
+		delete m_speedController;
+		delete m_encoder;
+	}
+	
+	void Reset() {
+		m_speedController->Set(0.0f);
+		m_encoder->Reset();
+		m_encoder->Start();
+	}
+};
+
+
+/**
+ * Test if the encoder value increments after the motor drives forward
+ */
+TEST_P(MotorEncoderTest, Increment) {
+	Reset();
+	
+	/* Drive the speed controller briefly to move the encoder */
+	m_speedController->Set(1.0);
+	Wait(kMotorTime);
+	m_speedController->Set(0.0);
+	
+	/* The encoder should be positive now */
+	EXPECT_GT(m_encoder->Get(), 0)
+		<< "Encoder should have incremented after the motor moved";
+}
+
+
+/**
+ * Test if the encoder value decrements after the motor drives backwards
+ */
+TEST_P(MotorEncoderTest, Decrement) {
+	Reset();
+	
+	/* Drive the speed controller briefly to move the encoder */
+	m_speedController->Set(-1.0f);
+	Wait(kMotorTime);
+	m_speedController->Set(0.0f);
+	
+	/* The encoder should be positive now */
+	EXPECT_LT(m_encoder->Get(), 0.0f)
+		<< "Encoder should have decremented after the motor moved";
+}
+
+/**
+ * Test if motor speeds are clamped to [-1,1]
+ */
+TEST_P(MotorEncoderTest, ClampSpeed) {
+	Reset();
+	
+	m_speedController->Set(2.0f);
+	Wait(kMotorTime);
+	
+	EXPECT_FLOAT_EQ(1.0f, m_speedController->Get());
+	
+	m_speedController->Set(-2.0f);
+	Wait(kMotorTime);
+	
+	EXPECT_FLOAT_EQ(-1.0f, m_speedController->Get());
+}
+
+/**
+ * Test if PID loops work
+ */
+TEST_P(MotorEncoderTest, PIDController) {
+	Reset();
+	
+	PIDController pid(0.003f, 0.001f, 0.0f, m_encoder, m_speedController);
+	pid.SetAbsoluteTolerance(15.0f);
+	pid.SetOutputRange(-0.2f, 0.2f);
+	pid.SetSetpoint(2500);
+	
+	/* 5 seconds should be plenty time to get to the setpoint */
+	pid.Enable();
+	Wait(5.0);
+	pid.Disable();
+	
+	RecordProperty("PID Error", pid.GetError());
+	
+	EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 5 seconds.";
+}
+
+/**
+ * Test resetting encoders
+ */
+TEST_P(MotorEncoderTest, Reset) {
+	Reset();
+	
+	EXPECT_EQ(0, m_encoder->Get()) << "Encoder did not reset to 0";
+}
+
+INSTANTIATE_TEST_CASE_P(Test, MotorEncoderTest,
+	testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON));
+