From 7041cbc5eb4bf70e4a9a9a8fb32e524e67122a1e Mon Sep 17 00:00:00 2001 From: Patrick Date: Thu, 28 Jan 2016 14:25:39 -0500 Subject: [PATCH] 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 --- .../src/MotorEncoderTest.cpp | 28 +++--- .../src/MotorInvertingTest.cpp | 2 +- .../src/PIDToleranceTest.cpp | 79 +++++++++++++++++ .../edu/wpi/first/wpilibj/PIDController.java | 13 +-- .../wpi/first/wpilibj/MotorEncoderTest.java | 18 ++-- .../wpi/first/wpilibj/MotorInvertingTest.java | 2 +- .../java/edu/wpi/first/wpilibj/PIDTest.java | 4 +- .../wpi/first/wpilibj/PIDToleranceTest.java | 88 +++++++++++++++++++ .../wpi/first/wpilibj/WpiLibJTestSuite.java | 4 +- .../wpilibj/can/CANJaguarInversionTest.java | 6 +- .../can/CANPercentQuadEncoderModeTest.java | 2 +- .../can/CANSpeedQuadEncoderModeTest.java | 2 +- 12 files changed, 207 insertions(+), 41 deletions(-) create mode 100644 wpilibcIntegrationTests/src/PIDToleranceTest.cpp create mode 100644 wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDToleranceTest.java 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() {