Fixed the motor tests by reducing speed to within the limits of the encoders we use. Also fixed java pid tolerances since getAvgError() was broken. It is now fixed and works properly. Added tests for both java and cpp that test if pid tolerances are working using fake input output pairs.

Change-Id: I5bf23dbbdab996c582e1035fc2b2f36dd5f52417

wpilibcIntegrationTests/src/MotorEncoderTest.cpp

@@ -84,7 +84,7 @@ TEST_P(MotorEncoderTest, Increment) {
   Reset();
 
   /* Drive the speed controller briefly to move the encoder */
-  m_speedController->Set(1.0);
+  m_speedController->Set(0.2f);
   Wait(kMotorTime);
   m_speedController->Set(0.0);
 
@@ -100,7 +100,7 @@ TEST_P(MotorEncoderTest, Decrement) {
   Reset();
 
   /* Drive the speed controller briefly to move the encoder */
-  m_speedController->Set(-1.0f);
+  m_speedController->Set(-0.2f);
   Wait(kMotorTime);
   m_speedController->Set(0.0f);
 
@@ -131,12 +131,12 @@ TEST_P(MotorEncoderTest, ClampSpeed) {
  */
 TEST_P(MotorEncoderTest, PositionPIDController) {
   Reset();
-
+  double goal = 1000;
   m_encoder->SetPIDSourceType(PIDSourceType::kDisplacement);
   PIDController pid(0.001f, 0.0005f, 0.0f, m_encoder, m_speedController);
-  pid.SetAbsoluteTolerance(20.0f);
-  pid.SetOutputRange(-0.3f, 0.3f);
-  pid.SetSetpoint(2500);
+  pid.SetAbsoluteTolerance(50.0f);
+  pid.SetOutputRange(-0.2f, 0.2f);
+  pid.SetSetpoint(goal);
 
   /* 10 seconds should be plenty time to get to the setpoint */
   pid.Enable();
@@ -145,7 +145,7 @@ TEST_P(MotorEncoderTest, PositionPIDController) {
 
   RecordProperty("PIDError", pid.GetError());
 
-  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds.";
+  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: "<<goal<<" Error was: "<<pid.GetError();
 }
 
 /**
@@ -156,20 +156,18 @@ TEST_P(MotorEncoderTest, VelocityPIDController) {
 
   m_encoder->SetPIDSourceType(PIDSourceType::kRate);
   PIDController pid(1e-5, 0.0f, 3e-5, 8e-5, m_encoder, m_speedController);
-  pid.SetAbsoluteTolerance(50.0f);
-  pid.SetToleranceBuffer(10);
+  pid.SetAbsoluteTolerance(200.0f);
+  pid.SetToleranceBuffer(50);
   pid.SetOutputRange(-0.3f, 0.3f);
-  pid.SetSetpoint(2000);
+  pid.SetSetpoint(600);
 
   /* 10 seconds should be plenty time to get to the setpoint */
   pid.Enable();
   Wait(10.0);
-
-  RecordProperty("PIDError", pid.GetAvgError());
-
-  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 2000 << " Error was: " << pid.GetError();
-
   pid.Disable();
+  RecordProperty("PIDError", pid.GetError());
+
+  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 600 << " Error was: " << pid.GetError();
 }
 
 /**

wpilibcIntegrationTests/src/MotorInvertingTest.cpp

@@ -14,7 +14,7 @@
 #include "TestBench.h"
 
 enum MotorInvertingTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
-static const double motorSpeed = 0.25;
+static const double motorSpeed = 0.15;
 static const double delayTime = 0.5;
 std::ostream &operator<<(std::ostream &os, MotorInvertingTestType const &type) {
   switch (type) {

wpilibcIntegrationTests/src/PIDToleranceTest.cpp

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-2016. 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 <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+#include "PIDSource.h"
+#include "PIDController.h"
+#include "PIDOutput.h"
+
+class PIDToleranceTest : public testing::Test{
+protected:
+	const double setpoint = 50.0;
+	const double range = 200;
+	const double tolerance = 10.0;
+	class fakeInput : public PIDSource{
+		public:
+		double val = 0;
+		void SetPIDSourceType(PIDSourceType pidSource){
+		  }
+		  PIDSourceType GetPIDSourceType(){
+			  return PIDSourceType::kDisplacement;
+		  }
+		  double PIDGet(){;
+		  	  return val;
+		  }
+	};
+	class fakeOutput : public PIDOutput{
+		void PIDWrite(float output){
+
+		}
+	};
+	fakeInput inp;
+	fakeOutput out;
+	PIDController *pid;
+	virtual void SetUp() override {
+		pid = new PIDController(0.5,0.0,0.0,&inp,&out);
+		pid->SetInputRange(-range/2,range/2);
+	}
+	virtual void TearDown() override {
+		delete pid;
+	}
+	virtual void reset(){
+		inp.val = 0;
+	}
+};
+
+TEST_F(PIDToleranceTest, Absolute){
+	reset();
+	pid->SetAbsoluteTolerance(tolerance);
+	pid->SetSetpoint(setpoint);
+	pid->Enable();
+	EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError();
+	inp.val = setpoint+tolerance/2;
+	Wait(1.0);
+	EXPECT_TRUE(pid->OnTarget())<<"Error was not in tolerance when it should have been. Error was " << pid->GetAvgError();
+	inp.val = setpoint+10*tolerance;
+	Wait(1.0);
+	EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError();
+}
+
+TEST_F(PIDToleranceTest, Percent){
+	reset();
+	pid->SetPercentTolerance(tolerance);
+	pid->SetSetpoint(setpoint);
+	pid->Enable();
+	EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError();
+	inp.val = setpoint+(tolerance)/200*range;//half of percent tolerance away from setpoint
+	Wait(1.0);
+	EXPECT_TRUE(pid->OnTarget())<<"Error was not in tolerance when it should have been. Error was " << pid->GetAvgError();
+	inp.val = setpoint+(tolerance)/50*range;//double percent tolerance away from setPoint
+	Wait(1.0);
+	EXPECT_FALSE(pid->OnTarget())<<"Error was in tolerance when it should not have been. Error was " << pid->GetAvgError();
+
+}