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

Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)

Browse Source
This commit is contained in:
Oblarg
2019-11-20 23:11:46 -05:00
committed by Peter Johnson
parent f62e23f1af
commit fa85fbfc1c
16 changed files with 320 additions and 448 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
View File

@@ -1,67 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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 "frc2/command/ProfiledPIDCommand.h"
using namespace frc2;
using State = frc::TrapezoidProfile::State;
ProfiledPIDCommand::ProfiledPIDCommand(
frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource,
std::function<State()> goalSource,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements)
: m_controller{controller},
m_measurement{std::move(measurementSource)},
m_goal{std::move(goalSource)},
m_useOutput{std::move(useOutput)} {
AddRequirements(requirements);
}
ProfiledPIDCommand::ProfiledPIDCommand(
frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource,
std::function<units::meter_t()> goalSource,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource,
[&goalSource]() {
return State{goalSource(), 0_mps};
},
useOutput, requirements) {}
ProfiledPIDCommand::ProfiledPIDCommand(
frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource, State goal,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
useOutput, requirements) {}
ProfiledPIDCommand::ProfiledPIDCommand(
frc::ProfiledPIDController controller,
std::function<units::meter_t()> measurementSource, units::meter_t goal,
std::function<void(double, State)> useOutput,
std::initializer_list<Subsystem*> requirements)
: ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
useOutput, requirements) {}
void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
void ProfiledPIDCommand::Execute() {
m_useOutput(m_controller.Calculate(m_measurement(), m_goal()),
m_controller.GetSetpoint());
}
void ProfiledPIDCommand::End(bool interrupted) {
m_useOutput(0, State{0_m, 0_mps});
}
frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
return m_controller;
}

36
wpilibNewCommands/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
View File

@@ -1,36 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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 "frc2/command/ProfiledPIDSubsystem.h"
using namespace frc2;
using State = frc::TrapezoidProfile::State;
ProfiledPIDSubsystem::ProfiledPIDSubsystem(
frc::ProfiledPIDController controller)
: m_controller{controller} {}
void ProfiledPIDSubsystem::Periodic() {
if (m_enabled) {
UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
m_controller.GetSetpoint());
}
}
void ProfiledPIDSubsystem::Enable() {
m_controller.Reset();
m_enabled = true;
}
void ProfiledPIDSubsystem::Disable() {
UseOutput(0, State{0_m, 0_mps});
m_enabled = false;
}
frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
return m_controller;
}

33
wpilibNewCommands/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
View File

@@ -1,33 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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 "frc2/command/TrapezoidProfileCommand.h"
using namespace frc2;
TrapezoidProfileCommand::TrapezoidProfileCommand(
frc::TrapezoidProfile profile,
std::function<void(frc::TrapezoidProfile::State)> output,
std::initializer_list<Subsystem*> requirements)
: m_profile(profile), m_output(output) {
AddRequirements(requirements);
}
void TrapezoidProfileCommand::Initialize() {
m_timer.Reset();
m_timer.Start();
}
void TrapezoidProfileCommand::Execute() {
m_output(m_profile.Calculate(m_timer.Get()));
}
void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
bool TrapezoidProfileCommand::IsFinished() {
return m_timer.HasPeriodPassed(m_profile.TotalTime());
}