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();
+
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java

@@ -6,7 +6,8 @@
 package edu.wpi.first.wpilibj;
 
 import java.util.TimerTask;
-import java.util.LinkedList;
+import java.util.ArrayDeque;
+import java.util.Queue;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
@@ -42,7 +43,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
   private Tolerance m_tolerance; // the tolerance object used to check if on
                                  // target
   private int m_bufLength = 1;
-  private LinkedList<Double> m_buf;
+  private Queue<Double> m_buf;
   private double m_bufTotal = 0.0;
   private double m_setpoint = 0.0;
   private double m_prevSetpoint = 0.0;
@@ -162,7 +163,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     HLUsageReporting.reportPIDController(instances);
     m_tolerance = new NullTolerance();
 
-    m_buf = new LinkedList<Double>();
+    m_buf = new ArrayDeque<Double>(m_bufLength+1);
   }
 
   /**
@@ -311,11 +312,11 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
         result = m_result;
 
         // Update the buffer.
-        m_buf.push(m_error);
+        m_buf.add(m_error);
         m_bufTotal += m_error;
         // Remove old elements when the buffer is full.
         if (m_buf.size() > m_bufLength) {
-          m_bufTotal -= m_buf.pop();
+          m_bufTotal -= m_buf.remove();
         }
       }
 
@@ -646,7 +647,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     // Cut the existing buffer down to size if needed.
     while (m_buf.size() > bufLength) {
-      m_bufTotal -= m_buf.pop();
+      m_bufTotal -= m_buf.remove();
     }
   }
 

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java

@@ -60,7 +60,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
   @Before
   public void setUp() {
     double initialSpeed = me.getMotor().get();
-    assertTrue(me.getType() + " Did not start with an initial speeed of 0 instead got: "
+    assertTrue(me.getType() + " Did not start with an initial speed of 0 instead got: "
         + initialSpeed, Math.abs(initialSpeed) < 0.001);
     me.setup();
 
@@ -101,7 +101,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
   public void testIncrement() {
     int startValue = me.getEncoder().get();
 
-    me.getMotor().set(.75);
+    me.getMotor().set(.2);
     Timer.delay(MOTOR_RUNTIME);
     int currentValue = me.getEncoder().get();
     assertTrue(me.getType() + " Encoder not incremented: start: " + startValue + "; current: "
@@ -117,7 +117,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
   public void testDecrement() {
     int startValue = me.getEncoder().get();
 
-    me.getMotor().set(-.75);
+    me.getMotor().set(-.2);
     Timer.delay(MOTOR_RUNTIME);
     int currentValue = me.getEncoder().get();
     assertTrue(me.getType() + " Encoder not decremented: start: " + startValue + "; current: "
@@ -170,18 +170,18 @@ public class MotorEncoderTest extends AbstractComsSetup {
   @Test
   public void testPositionPIDController() {
     me.getEncoder().setPIDSourceType(PIDSourceType.kDisplacement);
-    PIDController pid = new PIDController(0.003, 0.001, 0, me.getEncoder(), me.getMotor());
-    pid.setAbsoluteTolerance(50);
+    PIDController pid = new PIDController(0.001, 0.0005, 0, me.getEncoder(), me.getMotor());
+    pid.setAbsoluteTolerance(50.0);
     pid.setOutputRange(-0.2, 0.2);
-    pid.setSetpoint(2500);
+    pid.setSetpoint(1000);
 
     pid.enable();
     Timer.delay(10.0);
     pid.disable();
 
     assertTrue(
-        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getError(),
-        pid.onTarget());
+        "PID loop did not reach setpoint within 10 seconds. The average error was: " + pid.getAvgError() + 
+        "The current error was" + pid.getError(), pid.onTarget());
 
     pid.free();
   }
@@ -194,7 +194,7 @@ public class MotorEncoderTest extends AbstractComsSetup {
     pid.setAbsoluteTolerance(200);
     pid.setToleranceBuffer(50);
     pid.setOutputRange(-0.3, 0.3);
-    pid.setSetpoint(2000);
+    pid.setSetpoint(600);
 
     pid.enable();
     Timer.delay(10.0);

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java

@@ -30,7 +30,7 @@ import java.util.logging.Logger;
 @RunWith(Parameterized.class)
 public class MotorInvertingTest extends AbstractComsSetup {
   static MotorEncoderFixture<?> fixture = null;
-  private static final double motorspeed = 0.35;
+  private static final double motorspeed = 0.2;
   private static final double delaytime = 0.3;
 
 

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java

@@ -46,7 +46,7 @@ public class PIDTest extends AbstractComsSetup {
   private NetworkTable table;
 
   private static final double absoluteTolerance = 50;
-  private static final double outputRange = 0.3;
+  private static final double outputRange = 0.25;
 
   private PIDController controller = null;
   private static MotorEncoderFixture me = null;
@@ -181,7 +181,7 @@ public class PIDTest extends AbstractComsSetup {
   public void testRotateToTarget() {
     setupAbsoluteTolerance();
     setupOutputRange();
-    double setpoint = 2500.0;
+    double setpoint = 1000.0;
     assertEquals(pidData() + "did not start at 0", 0, controller.get(), 0);
     controller.setSetpoint(setpoint);
     assertEquals(pidData() + "did not have an error of " + setpoint, setpoint,

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDToleranceTest.java

@@ -0,0 +1,88 @@
+package edu.wpi.first.wpilibj;
+
+import java.util.logging.Logger;
+
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Test;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+
+public class PIDToleranceTest extends AbstractComsSetup {
+	private static final Logger logger = Logger.getLogger(PIDToleranceTest.class.getName());
+	private PIDController pid;
+	private final double setPoint = 50.0;
+	private final double tolerance = 10.0;
+	private final double range = 200;
+	private class fakeInput implements PIDSource{
+		public double val;
+		public fakeInput(){
+			val = 0;
+		}
+		@Override
+		public PIDSourceType getPIDSourceType() {
+			return PIDSourceType.kDisplacement;
+		}
+
+		@Override
+		public double pidGet() {
+			return val;
+		}
+
+		@Override
+		public void setPIDSourceType(PIDSourceType arg0) {}
+	};
+	private fakeInput inp;
+	private PIDOutput out = new PIDOutput(){
+		@Override
+		public void pidWrite(double out) {
+		}
+		
+	};
+	@Override
+	protected Logger getClassLogger() {
+		return logger;
+	}
+	@Before
+	public void setUp() throws Exception{
+		inp = new fakeInput();
+		pid = new PIDController(0.05,0.0,0.0,inp,out);
+		pid.setInputRange(-range/2, range/2);
+	}
+	
+	@After
+	public void tearDown() throws Exception{
+		pid.free();
+		pid = null;
+	}
+	
+	@Test
+	public void testAbsoluteTolerance(){
+		pid.setAbsoluteTolerance(tolerance);
+		pid.setSetpoint(setPoint);
+		pid.enable();
+		Timer.delay(1);
+		assertFalse("Error was in tolerance when it should not have been. Error was "+pid.getAvgError(),pid.onTarget());
+		inp.val = setPoint+tolerance/2;
+		Timer.delay(1.0);
+		assertTrue("Error was not in tolerance when it should have been. Error was "+pid.getAvgError(),pid.onTarget());
+		inp.val = setPoint + 10*tolerance;
+		Timer.delay(1.0);
+		assertFalse("Error was in tolerance when it should not have been. Error was "+pid.getAvgError(),pid.onTarget());
+	}
+	@Test
+	public void testPercentTolerance(){
+		pid.setPercentTolerance(tolerance);
+		pid.setSetpoint(setPoint);
+		pid.enable();
+		assertFalse("Error was in tolerance when it should not have been. Error was "+pid.getAvgError(),pid.onTarget());
+		inp.val = setPoint+(tolerance)/200*range;//half of percent tolerance away from setPoint
+		Timer.delay(1.0);
+		assertTrue("Error was not in tolerance when it should have been. Error was "+pid.getAvgError(),pid.onTarget());
+		inp.val = setPoint + (tolerance)/50*range;//double percent tolerance away from setPoint
+		Timer.delay(1.0);
+		assertFalse("Error was in tolerance when it should not have been. Error was "+pid.getAvgError(),pid.onTarget());
+	}
+}

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java

@@ -25,7 +25,7 @@ import edu.wpi.first.wpilibj.test.AbstractTestSuite;
     DIOCrossConnectTest.class, EncoderTest.class, FilterNoiseTest.class,
     FilterOutputTest.class, GyroTest.class, MotorEncoderTest.class,
     MotorInvertingTest.class, PCMTest.class, PDPTest.class, PIDTest.class,
-    PreferencesTest.class, RelayCrossConnectTest.class, SampleTest.class,
-    TimerTest.class})
+    PIDToleranceTest.class, PreferencesTest.class, RelayCrossConnectTest.class, 
+    SampleTest.class, TimerTest.class})
 public class WpiLibJTestSuite extends AbstractTestSuite {
 }

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java

@@ -18,9 +18,9 @@ import java.util.logging.Logger;
  */
 public class CANJaguarInversionTest extends AbstractCANTest {
   private static final Logger logger = Logger.getLogger(CANJaguarInversionTest.class.getName());
-  private static final double motorVoltage = 5.0;
-  private static final double motorPercent = 0.5;
-  private static final double motorSpeed = 100;
+  private static final double motorVoltage = 2.0;
+  private static final double motorPercent = 0.1;
+  private static final double motorSpeed = 10;
   private static final double delayTime = 0.75;
   private static final double speedModeDelayTime = 2.0;
 

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPercentQuadEncoderModeTest.java

@@ -37,7 +37,7 @@ public class CANPercentQuadEncoderModeTest extends AbstractCANTest {
   private static final Logger logger = Logger.getLogger(CANPercentQuadEncoderModeTest.class
       .getName());
   private static final double kStoppedValue = 0;
-  private static final double kRunningValue = 1;
+  private static final double kRunningValue = 0.3;
 
   /*
    * (non-Javadoc)

wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANSpeedQuadEncoderModeTest.java

@@ -31,7 +31,7 @@ public class CANSpeedQuadEncoderModeTest extends AbstractCANTest {
   /** The stopped value in rev/min */
   private static final double kStoppedValue = 0;
   /** The running value in rev/min */
-  private static final double kRunningValue = 200;
+  private static final double kRunningValue = 50;
 
   @Override
   protected Logger getClassLogger() {