Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/wpilibc/shared/src/Commands/PIDSubsystem.cpp b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
new file mode 100644
index 0000000..b3d327f
--- /dev/null
+++ b/wpilibc/shared/src/Commands/PIDSubsystem.cpp
@@ -0,0 +1,225 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2011. 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 $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/PIDSubsystem.h"
+#include "PIDController.h"
+#include "float.h"
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ */
+PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d,
+ double f)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values. It will also space the time
+ * between PID loop calculations to be equal to the given period.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedfoward value
+ * @param period the time (in seconds) between calculations
+ */
+PIDSubsystem::PIDSubsystem(const std::string &name, double p, double i, double d,
+ double f, double period)
+ : Subsystem(name) {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * It will use the class name as its name.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * It will use the class name as its name.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d
+ * values.
+ * It will use the class name as its name.
+ * It will also space the time
+ * between PID loop calculations to be equal to the given period.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ * @param period the time (in seconds) between calculations
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
+ double period)
+ : Subsystem("PIDSubsystem") {
+ m_controller = std::make_shared<PIDController>(p, i, d, f, this, this, period);
+}
+
+/**
+ * Enables the internal {@link PIDController}
+ */
+void PIDSubsystem::Enable() { m_controller->Enable(); }
+
+/**
+ * Disables the internal {@link PIDController}
+ */
+void PIDSubsystem::Disable() { m_controller->Disable(); }
+
+/**
+ * Returns the {@link PIDController} used by this {@link PIDSubsystem}.
+ * Use this if you would like to fine tune the pid loop.
+ *
+ * @return the {@link PIDController} used by this {@link PIDSubsystem}
+ */
+std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
+ return m_controller;
+}
+
+/**
+ * Sets the setpoint to the given value. If {@link PIDCommand#SetRange(double,
+ * double) SetRange(...)}
+ * was called,
+ * then the given setpoint
+ * will be trimmed to fit within the range.
+ * @param setpoint the new setpoint
+ */
+void PIDSubsystem::SetSetpoint(double setpoint) {
+ m_controller->SetSetpoint(setpoint);
+}
+
+/**
+ * Adds the given value to the setpoint.
+ * If {@link PIDCommand#SetRange(double, double) SetRange(...)} was used,
+ * then the bounds will still be honored by this method.
+ * @param deltaSetpoint the change in the setpoint
+ */
+void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+/**
+ * Return the current setpoint
+ * @return The current setpoint
+ */
+double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
+
+/**
+ * Sets the maximum and minimum values expected from the input.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the output
+ */
+void PIDSubsystem::SetInputRange(float minimumInput, float maximumInput) {
+ m_controller->SetInputRange(minimumInput, maximumInput);
+}
+
+/**
+ * Sets the maximum and minimum values to write.
+ *
+ * @param minimumOutput the minimum value to write to the output
+ * @param maximumOutput the maximum value to write to the output
+ */
+void PIDSubsystem::SetOutputRange(float minimumOutput, float maximumOutput) {
+ m_controller->SetOutputRange(minimumOutput, maximumOutput);
+}
+
+/*
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDSubsystem::SetAbsoluteTolerance(float absValue) {
+ m_controller->SetAbsoluteTolerance(absValue);
+}
+
+/*
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDSubsystem::SetPercentTolerance(float percent) {
+ m_controller->SetPercentTolerance(percent);
+}
+
+/*
+ * Return true if the error is within the percentage of the total input range,
+ * determined by SetTolerance. This asssumes that the maximum and minimum input
+ * were set using SetInput. Use OnTarget() in the IsFinished() method of
+ * commands
+ * that use this subsystem.
+ *
+ * Currently this just reports on target as the actual value passes through the
+ * setpoint.
+ * Ideally it should be based on being within the tolerance for some period of
+ * time.
+ *
+ * @return true if the error is within the percentage tolerance of the input
+ * range
+ */
+bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
+
+/**
+ * Returns the current position
+ * @return the current position
+ */
+double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
+
+/**
+ * Returns the current rate
+ * @return the current rate
+ */
+double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+
+void PIDSubsystem::PIDWrite(float output) { UsePIDOutput(output); }
+
+double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+
+std::string PIDSubsystem::GetSmartDashboardType() const { return "PIDCommand"; }
+void PIDSubsystem::InitTable(std::shared_ptr<ITable> table) {
+ m_controller->InitTable(table);
+ Subsystem::InitTable(table);
+}