Now you can download a copy of these docs so you can use them offline! Download now
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 = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
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 instances = 0;
98  instances++;
99  nUsageReporting::report(nUsageReporting::kResourceType_PIDController, instances);
100 
101  m_toleranceType = kNoTolerance;
102 }
103 
107 PIDController::~PIDController()
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 
131 void PIDController::Calculate()
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 
254 float PIDController::GetP()
255 {
256  CRITICAL_REGION(m_semaphore)
257  {
258  return m_P;
259  }
260  END_REGION;
261 }
262 
267 float PIDController::GetI()
268 {
269  CRITICAL_REGION(m_semaphore)
270  {
271  return m_I;
272  }
273  END_REGION;
274 }
275 
280 float PIDController::GetD()
281 {
282  CRITICAL_REGION(m_semaphore)
283  {
284  return m_D;
285  }
286  END_REGION;
287 }
288 
293 float PIDController::GetF()
294 {
295  CRITICAL_REGION(m_semaphore)
296  {
297  return m_F;
298  }
299  END_REGION;
300 }
301 
307 float PIDController::Get()
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 
402 float PIDController::GetSetpoint()
403 {
404  float setpoint;
405  CRITICAL_REGION(m_semaphore)
406  {
407  setpoint = m_setpoint;
408  }
409  END_REGION;
410  return setpoint;
411 }
412 
417 float PIDController::GetError()
418 {
419  float error;
420  CRITICAL_REGION(m_semaphore)
421  {
422  error = GetSetpoint() - 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 
504 void PIDController::Enable()
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 
520 void PIDController::Disable()
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 
537 bool PIDController::IsEnabled()
538 {
539  bool enabled;
540  CRITICAL_REGION(m_semaphore)
541  {
542  enabled = m_enabled;
543  }
544  END_REGION;
545  return enabled;
546 }
547 
551 void PIDController::Reset()
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 
568 void PIDController::InitTable(ITable* table){
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 }
Generated on Tue Feb 5 2013 00:55:09 for WPILibC++ by doxygen 1.8.1.2