Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix ProfiledPIDController profile direction for continuous input (#2279)

Browse Source 
Previously, it could take the long way around. This recomputes the
profile goal with the shortest error, thus taking the shortest path.

Also removed the setpoint clamping from PIDController::SetSetpoint()
because it's unnecessary to make PIDController behave correctly for
a modular arithmetic input, and it breaks the setpoint calculation in
ProfiledPIDController otherwise.

Fixes #2277.
This commit is contained in:
Tyler Veness
2020-03-14 22:13:57 -07:00
committed by GitHub
parent 8edf9282c3
commit 84e300739c
9 changed files with 233 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp Normal file
View File

@@ -0,0 +1,37 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. 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 <units/units.h>
#include "frc/controller/ControllerUtil.h"
#include "gtest/gtest.h"
TEST(ControllerUtilTest, GetModulusError) {
// Test symmetric range
EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -180.0, 180.0));
EXPECT_FLOAT_EQ(-20.0,
frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
EXPECT_FLOAT_EQ(-20.0,
frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
// Test range starting at zero
EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0));
EXPECT_FLOAT_EQ(-20.0,
frc::GetModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
EXPECT_FLOAT_EQ(-20.0,
frc::GetModulusError(170.0, 190.0 + 360.0, 0.0, 360.0));
// Test asymmetric range that doesn't start at zero
EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -170.0, 190.0));
// Test all supported types
EXPECT_FLOAT_EQ(-20.0,
frc::GetModulusError<double>(170.0, -170.0, -170.0, 190.0));
EXPECT_EQ(-20, frc::GetModulusError<int>(170, -170, -170, 190));
EXPECT_EQ(-20_deg, frc::GetModulusError<units::degree_t>(170_deg, -170_deg,
-170_deg, 190_deg));
}