mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
5598445a67
See https://usfirst.collab.net/sf/tracker/do/viewArtifact/projects.wpilib/tracker.4_defects/artf4148 Change-Id: I47b0d5a91fd49e47e2c7348b0705e998ec815682
110 lines
3.6 KiB
C++
110 lines
3.6 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) FIRST 2008. 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. */
|
|
/*----------------------------------------------------------------------------*/
|
|
#pragma once
|
|
|
|
#include "Base.h"
|
|
#include "Controller.h"
|
|
#include "LiveWindow/LiveWindow.h"
|
|
#include "HAL/Semaphore.hpp"
|
|
|
|
class PIDOutput;
|
|
class PIDSource;
|
|
class Notifier;
|
|
|
|
/**
|
|
* Class implements a PID Control Loop.
|
|
*
|
|
* Creates a separate thread which reads the given PIDSource and takes
|
|
* care of the integral calculations, as well as writing the given
|
|
* PIDOutput
|
|
*/
|
|
class PIDController : public LiveWindowSendable, public Controller, public ITableListener
|
|
{
|
|
public:
|
|
PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output, float period =
|
|
0.05);
|
|
PIDController(float p, float i, float d, float f, PIDSource *source, PIDOutput *output,
|
|
float period = 0.05);
|
|
virtual ~PIDController();
|
|
virtual float Get() const;
|
|
virtual void SetContinuous(bool continuous = true);
|
|
virtual void SetInputRange(float minimumInput, float maximumInput);
|
|
virtual void SetOutputRange(float minimumOutput, float maximumOutput);
|
|
virtual void SetPID(float p, float i, float d);
|
|
virtual void SetPID(float p, float i, float d, float f);
|
|
virtual float GetP() const;
|
|
virtual float GetI() const;
|
|
virtual float GetD() const;
|
|
virtual float GetF() const;
|
|
|
|
virtual void SetSetpoint(float setpoint);
|
|
virtual float GetSetpoint() const;
|
|
|
|
virtual float GetError() const;
|
|
|
|
virtual void SetTolerance(float percent);
|
|
virtual void SetAbsoluteTolerance(float absValue);
|
|
virtual void SetPercentTolerance(float percentValue);
|
|
virtual bool OnTarget() const;
|
|
|
|
virtual void Enable() override;
|
|
virtual void Disable() override;
|
|
virtual bool IsEnabled() const;
|
|
|
|
virtual void Reset();
|
|
|
|
virtual void InitTable(ITable* table);
|
|
|
|
private:
|
|
float m_P; // factor for "proportional" control
|
|
float m_I; // factor for "integral" control
|
|
float m_D; // factor for "derivative" control
|
|
float m_F; // factor for "feed forward" control
|
|
float m_maximumOutput; // |maximum output|
|
|
float m_minimumOutput; // |minimum output|
|
|
float m_maximumInput; // maximum input - limit setpoint to this
|
|
float m_minimumInput; // minimum input - limit setpoint to this
|
|
bool m_continuous; // do the endpoints wrap around? eg. Absolute encoder
|
|
bool m_enabled; //is the pid controller enabled
|
|
bool m_destruct; // should the calculate thread stop running
|
|
float m_prevError; // the prior sensor input (used to compute velocity)
|
|
double m_totalError; //the sum of the errors for use in the integral calc
|
|
enum
|
|
{
|
|
kAbsoluteTolerance,
|
|
kPercentTolerance,
|
|
kNoTolerance
|
|
} m_toleranceType;
|
|
float m_tolerance; //the percetage or absolute error that is considered on target
|
|
float m_setpoint;
|
|
float m_error;
|
|
float m_result;
|
|
float m_period;
|
|
|
|
MUTEX_ID m_semaphore;
|
|
|
|
PIDSource *m_pidInput;
|
|
PIDOutput *m_pidOutput;
|
|
|
|
Notifier* m_controlLoop;
|
|
|
|
void Initialize(float p, float i, float d, float f, PIDSource *source, PIDOutput *output,
|
|
float period = 0.05);
|
|
static void CallCalculate(void *controller);
|
|
|
|
virtual ITable* GetTable() const;
|
|
virtual std::string GetSmartDashboardType() const;
|
|
virtual void ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew);
|
|
virtual void UpdateTable();
|
|
virtual void StartLiveWindowMode();
|
|
virtual void StopLiveWindowMode();
|
|
protected:
|
|
ITable* m_table;
|
|
virtual void Calculate();
|
|
|
|
DISALLOW_COPY_AND_ASSIGN(PIDController);
|
|
};
|