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

Fix trapezoidal profile PID controller setpoint bug (#2210)

Browse Source 
Co-Authored-By: Austin Shalit <austinshalit@gmail.com>
This commit is contained in:
Matt
2020-01-01 15:23:25 -08:00
committed by Peter Johnson
parent abe25b795b
commit 222669dc2c
8 changed files with 90 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
View File

@@ -328,7 +328,7 @@ public class PIDController implements Sendable, AutoCloseable {
}
/**
* Resets the previous error and the integral term. Also disables the controller.
* Resets the previous error and the integral term.
*/
public void reset() {
m_prevError = 0;

32
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Copyright (c) 2019-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. */
@@ -15,7 +15,8 @@ import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
/**
* Implements a PID control loop whose setpoint is constrained by a trapezoid
* profile.
* profile. Users should call reset() when they first start running the controller
* to avoid unwanted behavior.
*/
@SuppressWarnings("PMD.TooManyMethods")
public class ProfiledPIDController implements Sendable {
@@ -319,10 +320,33 @@ public class ProfiledPIDController implements Sendable {
}
/**
* Reset the previous error, the integral term, and disable the controller.
* Reset the previous error and the integral term.
*
* @param measurement The current measured State of the system.
*/
public void reset() {
public void reset(TrapezoidProfile.State measurement) {
m_controller.reset();
m_setpoint = measurement;
}
/**
* Reset the previous error and the integral term.
*
* @param measuredPosition The current measured position of the system.
* @param measuredVelocity The current measured velocity of the system.
*/
public void reset(double measuredPosition, double measuredVelocity) {
reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
}
/**
* Reset the previous error and the integral term.
*
* @param measuredPosition The current measured position of the system. The velocity is
* assumed to be zero.
*/
public void reset(double measuredPosition) {
reset(measuredPosition, 0.0);
}
@Override