WPILibC++  trunk
PIDController.cpp
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) FIRST 2008. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
5 /*----------------------------------------------------------------------------*/
6 
7 #include "PIDController.h"
8 #include "NetworkCommunication/UsageReporting.h"
9 #include "Notifier.h"
10 #include "PIDSource.h"
11 #include "PIDOutput.h"
12 #include <math.h>
13 #include "Synchronized.h"
14 
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";
21 
22 
33 PIDController::PIDController(float Kp, float Ki, float Kd,
34  PIDSource *source, PIDOutput *output,
35  float period) :
36  m_semaphore (0)
37 {
38  Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
39 }
40 
51 PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
52  PIDSource *source, PIDOutput *output,
53  float period) :
54  m_semaphore (0)
55 {
56  Initialize(Kp, Ki, Kd, Kf, source, output, period);
57 }
58 
59 
60 void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
61  PIDSource *source, PIDOutput *output,
62  float period)
63 {
64  m_table = NULL;
65 
66  m_semaphore = semMCreate(SEM_Q_PRIORITY);
67 
68  m_controlLoop = new Notifier(PIDController::CallCalculate, this);
69 
70  m_P = Kp;
71  m_I = Ki;
72  m_D = Kd;
73  m_F = Kf;
74 
75  m_maximumOutput = 1.0;
76  m_minimumOutput = -1.0;
77 
78  m_maximumInput = 0;
79  m_minimumInput = 0;
80 
81  m_continuous = false;
82  m_enabled = false;
83  m_setpoint = 0;
84 
85  m_prevError = 0;
86  m_totalError = 0;
87  m_tolerance = .05;
88 
89  m_result = 0;
90 
91  m_pidInput = source;
92  m_pidOutput = output;
93  m_period = period;
94 
95  m_controlLoop->StartPeriodic(m_period);
96 
97  static int32_t instances = 0;
98  instances++;
99  nUsageReporting::report(nUsageReporting::kResourceType_PIDController, instances);
100 
101  m_toleranceType = kNoTolerance;
102 }
103 
108 {
109  semFlush(m_semaphore);
110  delete m_controlLoop;
111 }
112 
120 void PIDController::CallCalculate(void *controller)
121 {
122  PIDController *control = (PIDController*) controller;
123  control->Calculate();
124 }
125 
132 {
133  bool enabled;
134  PIDSource *pidInput;
135 
136  CRITICAL_REGION(m_semaphore)
137  {
138  if (m_pidInput == 0) return;
139  if (m_pidOutput == 0) return;
140  enabled = m_enabled;
141  pidInput = m_pidInput;
142  }
143  END_REGION;
144 
145  if (enabled)
146  {
147  float input = pidInput->PIDGet();
148  float result;
149  PIDOutput *pidOutput;
150 
151  {
152  Synchronized sync(m_semaphore);
153  m_error = m_setpoint - input;
154  if (m_continuous)
155  {
156  if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2)
157  {
158  if (m_error > 0)
159  {
160  m_error = m_error - m_maximumInput + m_minimumInput;
161  }
162  else
163  {
164  m_error = m_error + m_maximumInput - m_minimumInput;
165  }
166  }
167  }
168 
169  if(m_I != 0)
170  {
171  double potentialIGain = (m_totalError + m_error) * m_I;
172  if (potentialIGain < m_maximumOutput)
173  {
174  if (potentialIGain > m_minimumOutput)
175  m_totalError += m_error;
176  else
177  m_totalError = m_minimumOutput / m_I;
178  }
179  else
180  {
181  m_totalError = m_maximumOutput / m_I;
182  }
183  }
184 
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;
187 
188  if (m_result > m_maximumOutput) m_result = m_maximumOutput;
189  else if (m_result < m_minimumOutput) m_result = m_minimumOutput;
190 
191  pidOutput = m_pidOutput;
192  result = m_result;
193  }
194 
195  pidOutput->PIDWrite(result);
196  }
197 }
198 
206 void PIDController::SetPID(float p, float i, float d)
207 {
208  CRITICAL_REGION(m_semaphore)
209  {
210  m_P = p;
211  m_I = i;
212  m_D = d;
213  }
214  END_REGION;
215 
216  if (m_table != NULL) {
217  m_table->PutNumber("p", m_P);
218  m_table->PutNumber("i", m_I);
219  m_table->PutNumber("d", m_D);
220  }
221 }
222 
231 void PIDController::SetPID(float p, float i, float d, float f)
232 {
233  CRITICAL_REGION(m_semaphore)
234  {
235  m_P = p;
236  m_I = i;
237  m_D = d;
238  m_F = f;
239  }
240  END_REGION;
241 
242  if (m_table != NULL) {
243  m_table->PutNumber("p", m_P);
244  m_table->PutNumber("i", m_I);
245  m_table->PutNumber("d", m_D);
246  m_table->PutNumber("f", m_F);
247  }
248 }
249 
255 {
256  CRITICAL_REGION(m_semaphore)
257  {
258  return m_P;
259  }
260  END_REGION;
261 }
262 
268 {
269  CRITICAL_REGION(m_semaphore)
270  {
271  return m_I;
272  }
273  END_REGION;
274 }
275 
281 {
282  CRITICAL_REGION(m_semaphore)
283  {
284  return m_D;
285  }
286  END_REGION;
287 }
288 
294 {
295  CRITICAL_REGION(m_semaphore)
296  {
297  return m_F;
298  }
299  END_REGION;
300 }
301 
308 {
309  float result;
310  CRITICAL_REGION(m_semaphore)
311  {
312  result = m_result;
313  }
314  END_REGION;
315  return result;
316 }
317 
325 void PIDController::SetContinuous(bool continuous)
326 {
327  CRITICAL_REGION(m_semaphore)
328  {
329  m_continuous = continuous;
330  }
331  END_REGION;
332 
333 }
334 
341 void PIDController::SetInputRange(float minimumInput, float maximumInput)
342 {
343  CRITICAL_REGION(m_semaphore)
344  {
345  m_minimumInput = minimumInput;
346  m_maximumInput = maximumInput;
347  }
348  END_REGION;
349 
350  SetSetpoint(m_setpoint);
351 }
352 
359 void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
360 {
361  CRITICAL_REGION(m_semaphore)
362  {
363  m_minimumOutput = minimumOutput;
364  m_maximumOutput = maximumOutput;
365  }
366  END_REGION;
367 }
368 
373 void PIDController::SetSetpoint(float setpoint)
374 {
375  CRITICAL_REGION(m_semaphore)
376  {
377  if (m_maximumInput > m_minimumInput)
378  {
379  if (setpoint > m_maximumInput)
380  m_setpoint = m_maximumInput;
381  else if (setpoint < m_minimumInput)
382  m_setpoint = m_minimumInput;
383  else
384  m_setpoint = setpoint;
385  }
386  else
387  {
388  m_setpoint = setpoint;
389  }
390  }
391  END_REGION;
392 
393  if (m_table != NULL) {
394  m_table->PutNumber("setpoint", m_setpoint);
395  }
396 }
397 
403 {
404  float setpoint;
405  CRITICAL_REGION(m_semaphore)
406  {
407  setpoint = m_setpoint;
408  }
409  END_REGION;
410  return setpoint;
411 }
412 
418 {
419  float error;
420  CRITICAL_REGION(m_semaphore)
421  {
422  error = m_setpoint - m_pidInput->PIDGet();
423  }
424  END_REGION;
425  return error;
426 }
427 
428 /*
429  * Set the percentage error which is considered tolerable for use with
430  * OnTarget.
431  * @param percentage error which is tolerable
432  */
433 void PIDController::SetTolerance(float percent)
434 {
435  CRITICAL_REGION(m_semaphore)
436  {
437  m_toleranceType = kPercentTolerance;
438  m_tolerance = percent;
439  }
440  END_REGION;
441 }
442 
443 /*
444  * Set the percentage error which is considered tolerable for use with
445  * OnTarget.
446  * @param percentage error which is tolerable
447  */
448 void PIDController::SetPercentTolerance(float percent)
449 {
450  CRITICAL_REGION(m_semaphore)
451  {
452  m_toleranceType = kPercentTolerance;
453  m_tolerance = percent;
454  }
455  END_REGION;
456 }
457 
458 /*
459  * Set the absolute error which is considered tolerable for use with
460  * OnTarget.
461  * @param percentage error which is tolerable
462  */
463 void PIDController::SetAbsoluteTolerance(float absTolerance)
464 {
465  CRITICAL_REGION(m_semaphore)
466  {
467  m_toleranceType = kAbsoluteTolerance;
468  m_tolerance = absTolerance;
469  }
470  END_REGION;
471 }
472 
473 /*
474  * Return true if the error is within the percentage of the total input range,
475  * determined by SetTolerance. This asssumes that the maximum and minimum input
476  * were set using SetInput.
477  * Currently this just reports on target as the actual value passes through the setpoint.
478  * Ideally it should be based on being within the tolerance for some period of time.
479  */
480 bool PIDController::OnTarget()
481 {
482  bool temp;
483  CRITICAL_REGION(m_semaphore)
484  {
485  switch (m_toleranceType) {
486  case kPercentTolerance:
487  temp = fabs(GetError()) < (m_tolerance / 100 * (m_maximumInput - m_minimumInput));
488  break;
489  case kAbsoluteTolerance:
490  temp = fabs(GetError()) < m_tolerance;
491  break;
492  //TODO: this case needs an error
493  case kNoTolerance:
494  temp = false;
495  }
496  }
497  END_REGION;
498  return temp;
499 }
500 
505 {
506  CRITICAL_REGION(m_semaphore)
507  {
508  m_enabled = true;
509  }
510  END_REGION;
511 
512  if (m_table != NULL) {
513  m_table->PutBoolean("enabled", true);
514  }
515 }
516 
521 {
522  CRITICAL_REGION(m_semaphore)
523  {
524  m_pidOutput->PIDWrite(0);
525  m_enabled = false;
526  }
527  END_REGION;
528 
529  if (m_table != NULL) {
530  m_table->PutBoolean("enabled", false);
531  }
532 }
533 
538 {
539  bool enabled;
540  CRITICAL_REGION(m_semaphore)
541  {
542  enabled = m_enabled;
543  }
544  END_REGION;
545  return enabled;
546 }
547 
552 {
553  Disable();
554 
555  CRITICAL_REGION(m_semaphore)
556  {
557  m_prevError = 0;
558  m_totalError = 0;
559  m_result = 0;
560  }
561  END_REGION;
562 }
563 
564 std::string PIDController::GetSmartDashboardType(){
565  return "PIDController";
566 }
567 
569  if(m_table!=NULL)
570  m_table->RemoveTableListener(this);
571  m_table = table;
572  if(m_table!=NULL){
573  m_table->PutNumber(kP, GetP());
574  m_table->PutNumber(kI, GetI());
575  m_table->PutNumber(kD, GetD());
576  m_table->PutNumber(kF, GetF());
577  m_table->PutNumber(kSetpoint, GetSetpoint());
578  m_table->PutBoolean(kEnabled, IsEnabled());
579  m_table->AddTableListener(this, false);
580  }
581 }
582 
583 ITable* PIDController::GetTable(){
584  return m_table;
585 }
586 
587 void PIDController::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew){
588  if (key==kP || key==kI || key==kD || key==kF) {
589  if (m_P != m_table->GetNumber(kP) || m_I != m_table->GetNumber(kI) || m_D != m_table->GetNumber(kD) || m_F != m_table->GetNumber(kF) ) {
590  SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0), m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
591  }
592  } else if (key==kSetpoint && m_setpoint != value.f) {
593  SetSetpoint(value.f);
594  } else if (key==kEnabled && m_enabled != value.b) {
595  if (value.b) {
596  Enable();
597  } else {
598  Disable();
599  }
600  }
601 }
602 
603 void PIDController::UpdateTable() {
604 
605 }
606 
607 void PIDController::StartLiveWindowMode() {
608  Disable();
609 }
610 
611 void PIDController::StopLiveWindowMode() {
612 
613 }
virtual void RemoveTableListener(ITableListener *listener)=0
virtual float GetP()
virtual void Reset()
virtual void PutNumber(std::string key, double value)=0
virtual float Get()
virtual void SetSetpoint(float setpoint)
Definition: ITable.h:26
virtual void AddTableListener(ITableListener *listener)=0
void StartPeriodic(double period)
Definition: Notifier.cpp:249
virtual void Enable()
virtual float GetError()
virtual void SetContinuous(bool continuous=true)
virtual float GetD()
virtual void PutBoolean(std::string key, bool value)=0
virtual float GetI()
PIDController(float p, float i, float d, PIDSource *source, PIDOutput *output, float period=0.05)
virtual double GetNumber(std::string key)=0
virtual ~PIDController()
virtual void InitTable(ITable *table)
virtual void SetPID(float p, float i, float d)
virtual bool IsEnabled()
virtual float GetF()
virtual float GetSetpoint()
virtual void Disable()
virtual void SetInputRange(float minimumInput, float maximumInput)
virtual void SetOutputRange(float mimimumOutput, float maximumOutput)
Definition: ITable.h:13