diff --git a/wpilibcIntegrationTests/src/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/MotorEncoderTest.cpp index cbec13f43d..9b3c609979 100644 --- a/wpilibcIntegrationTests/src/MotorEncoderTest.cpp +++ b/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: "<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(); } /** diff --git a/wpilibcIntegrationTests/src/MotorInvertingTest.cpp b/wpilibcIntegrationTests/src/MotorInvertingTest.cpp index 248ecc8c92..fc78c32cee 100644 --- a/wpilibcIntegrationTests/src/MotorInvertingTest.cpp +++ b/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) { diff --git a/wpilibcIntegrationTests/src/PIDToleranceTest.cpp b/wpilibcIntegrationTests/src/PIDToleranceTest.cpp new file mode 100644 index 0000000000..35e60ae583 --- /dev/null +++ b/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 +#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(); + +} diff --git a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java index 68a8c66289..4e4acd0332 100644 --- a/wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDController.java +++ b/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 m_buf; + private Queue 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(); + m_buf = new ArrayDeque(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(); } } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java index 398b4a5b98..b5c68253a7 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java +++ b/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); diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java index 5785d83e3d..0245ca101f 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java +++ b/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; diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java index c75426c0b4..948019e64c 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java +++ b/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, diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDToleranceTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDToleranceTest.java new file mode 100644 index 0000000000..12059afa53 --- /dev/null +++ b/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()); + } +} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java index 32235a3456..6ef4e6f0f5 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java +++ b/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 { } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java index b7dbcba64f..becd5f9153 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java +++ b/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; diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPercentQuadEncoderModeTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPercentQuadEncoderModeTest.java index 0d4ceae17d..03e72540d1 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPercentQuadEncoderModeTest.java +++ b/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) diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANSpeedQuadEncoderModeTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANSpeedQuadEncoderModeTest.java index e38a474906..0a98b3112f 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANSpeedQuadEncoderModeTest.java +++ b/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() {