mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
80 lines
2.6 KiB
C++
80 lines
2.6 KiB
C++
|
/*----------------------------------------------------------------------------*/
|
||
|
|
/* 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();
|
||
|
|
|
||
|
|
}
|