7 #include "PIDController.h"
8 #include "NetworkCommunication/UsageReporting.h"
10 #include "PIDSource.h"
11 #include "PIDOutput.h"
13 #include "Synchronized.h"
15 static const char *kP =
"p";
16 static const char *kI =
"i";
17 static const char *kD =
"d";
18 static const char *kF =
"f";
19 static const char *kSetpoint =
"setpoint";
20 static const char *kEnabled =
"enabled";
38 Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
56 Initialize(Kp, Ki, Kd, Kf, source, output, period);
60 void PIDController::Initialize(
float Kp,
float Ki,
float Kd,
float Kf,
66 m_semaphore = semMCreate(SEM_Q_PRIORITY);
68 m_controlLoop =
new Notifier(PIDController::CallCalculate,
this);
75 m_maximumOutput = 1.0;
76 m_minimumOutput = -1.0;
97 static int32_t instances = 0;
99 nUsageReporting::report(nUsageReporting::kResourceType_PIDController, instances);
101 m_toleranceType = kNoTolerance;
109 semFlush(m_semaphore);
110 delete m_controlLoop;
120 void PIDController::CallCalculate(
void *controller)
136 CRITICAL_REGION(m_semaphore)
138 if (m_pidInput == 0)
return;
139 if (m_pidOutput == 0)
return;
141 pidInput = m_pidInput;
147 float input = pidInput->PIDGet();
153 m_error = m_setpoint - input;
156 if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2)
160 m_error = m_error - m_maximumInput + m_minimumInput;
164 m_error = m_error + m_maximumInput - m_minimumInput;
171 double potentialIGain = (m_totalError + m_error) * m_I;
172 if (potentialIGain < m_maximumOutput)
174 if (potentialIGain > m_minimumOutput)
175 m_totalError += m_error;
177 m_totalError = m_minimumOutput / m_I;
181 m_totalError = m_maximumOutput / m_I;
185 m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F;
186 m_prevError = m_error;
188 if (m_result > m_maximumOutput) m_result = m_maximumOutput;
189 else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
191 pidOutput = m_pidOutput;
195 pidOutput->PIDWrite(result);
208 CRITICAL_REGION(m_semaphore)
216 if (m_table != NULL) {
233 CRITICAL_REGION(m_semaphore)
242 if (m_table != NULL) {
256 CRITICAL_REGION(m_semaphore)
269 CRITICAL_REGION(m_semaphore)
282 CRITICAL_REGION(m_semaphore)
295 CRITICAL_REGION(m_semaphore)
310 CRITICAL_REGION(m_semaphore)
327 CRITICAL_REGION(m_semaphore)
329 m_continuous = continuous;
343 CRITICAL_REGION(m_semaphore)
345 m_minimumInput = minimumInput;
346 m_maximumInput = maximumInput;
361 CRITICAL_REGION(m_semaphore)
363 m_minimumOutput = minimumOutput;
364 m_maximumOutput = maximumOutput;
375 CRITICAL_REGION(m_semaphore)
377 if (m_maximumInput > m_minimumInput)
379 if (setpoint > m_maximumInput)
380 m_setpoint = m_maximumInput;
381 else if (setpoint < m_minimumInput)
382 m_setpoint = m_minimumInput;
384 m_setpoint = setpoint;
388 m_setpoint = setpoint;
393 if (m_table != NULL) {
394 m_table->
PutNumber(
"setpoint", m_setpoint);
405 CRITICAL_REGION(m_semaphore)
407 setpoint = m_setpoint;
420 CRITICAL_REGION(m_semaphore)
422 error = m_setpoint - m_pidInput->PIDGet();
433 void PIDController::SetTolerance(
float percent)
435 CRITICAL_REGION(m_semaphore)
437 m_toleranceType = kPercentTolerance;
438 m_tolerance = percent;
448 void PIDController::SetPercentTolerance(
float percent)
450 CRITICAL_REGION(m_semaphore)
452 m_toleranceType = kPercentTolerance;
453 m_tolerance = percent;
463 void PIDController::SetAbsoluteTolerance(
float absTolerance)
465 CRITICAL_REGION(m_semaphore)
467 m_toleranceType = kAbsoluteTolerance;
468 m_tolerance = absTolerance;
480 bool PIDController::OnTarget()
483 CRITICAL_REGION(m_semaphore)
485 switch (m_toleranceType) {
486 case kPercentTolerance:
487 temp = fabs(
GetError()) < (m_tolerance / 100 * (m_maximumInput - m_minimumInput));
489 case kAbsoluteTolerance:
490 temp = fabs(
GetError()) < m_tolerance;
506 CRITICAL_REGION(m_semaphore)
512 if (m_table != NULL) {
522 CRITICAL_REGION(m_semaphore)
524 m_pidOutput->PIDWrite(0);
529 if (m_table != NULL) {
540 CRITICAL_REGION(m_semaphore)
555 CRITICAL_REGION(m_semaphore)
564 std::string PIDController::GetSmartDashboardType(){
565 return "PIDController";
583 ITable* PIDController::GetTable(){
587 void PIDController::ValueChanged(
ITable* source,
const std::string& key,
EntryValue value,
bool isNew){
588 if (key==kP || key==kI || key==kD || key==kF) {
592 }
else if (key==kSetpoint && m_setpoint != value.f) {
594 }
else if (key==kEnabled && m_enabled != value.b) {
603 void PIDController::UpdateTable() {
607 void PIDController::StartLiveWindowMode() {
611 void PIDController::StopLiveWindowMode() {
virtual void RemoveTableListener(ITableListener *listener)=0
virtual void PutNumber(std::string key, double value)=0
virtual void SetSetpoint(float setpoint)
virtual void AddTableListener(ITableListener *listener)=0
void StartPeriodic(double period)
virtual void SetContinuous(bool continuous=true)
virtual void PutBoolean(std::string key, bool value)=0
PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output, float period=0.05)
virtual double GetNumber(std::string key)=0
virtual void InitTable(ITable *table)
virtual void SetPID(float p, float i, float d)
virtual float GetSetpoint()
virtual void SetInputRange(float minimumInput, float maximumInput)
virtual void SetOutputRange(float mimimumOutput, float maximumOutput)