mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
8f402645f5
No longer stores a temporary setpoint in PIDSubsystem, instead immediately sending to PIDController. This fixes an issue where the setpoint didn't take effect until the Subsystem Periodic method ran, and could cause commands to finish early if they were scheduled after the subsystem periodic method ran because it used the old setpoint.
48 lines
1.0 KiB
C++
48 lines
1.0 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include "frc2/command/PIDSubsystem.h"
|
|
|
|
#include <utility>
|
|
|
|
using namespace frc2;
|
|
|
|
PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
|
|
: m_controller{std::move(controller)} {
|
|
SetSetpoint(initialPosition);
|
|
AddChild("PID Controller", &m_controller);
|
|
}
|
|
|
|
void PIDSubsystem::Periodic() {
|
|
if (m_enabled) {
|
|
UseOutput(m_controller.Calculate(GetMeasurement()), GetSetpoint());
|
|
}
|
|
}
|
|
|
|
void PIDSubsystem::SetSetpoint(double setpoint) {
|
|
m_controller.SetSetpoint(setpoint);
|
|
}
|
|
|
|
double PIDSubsystem::GetSetpoint() const {
|
|
return m_controller.GetSetpoint();
|
|
}
|
|
|
|
void PIDSubsystem::Enable() {
|
|
m_controller.Reset();
|
|
m_enabled = true;
|
|
}
|
|
|
|
void PIDSubsystem::Disable() {
|
|
UseOutput(0, 0);
|
|
m_enabled = false;
|
|
}
|
|
|
|
bool PIDSubsystem::IsEnabled() {
|
|
return m_enabled;
|
|
}
|
|
|
|
PIDController& PIDSubsystem::GetController() {
|
|
return m_controller;
|
|
}
|