From c937c409aeccacd9d90453f072a1b88e81b4cc99 Mon Sep 17 00:00:00 2001 From: thomasclark Date: Thu, 5 Jun 2014 11:32:49 -0400 Subject: [PATCH] Made PIDController use pthreads in C++ Change-Id: I3d4d1aa912bf56faa1d5e150732da5e7b551077d --- wpilibc/wpilibC++/include/PIDController.h | 11 +- wpilibc/wpilibC++/lib/PIDController.cpp | 408 +++++++++++----------- 2 files changed, 209 insertions(+), 210 deletions(-) diff --git a/wpilibc/wpilibC++/include/PIDController.h b/wpilibc/wpilibC++/include/PIDController.h index 060defd0eb..ea96791dc3 100644 --- a/wpilibc/wpilibC++/include/PIDController.h +++ b/wpilibc/wpilibC++/include/PIDController.h @@ -6,9 +6,9 @@ #pragma once #include "Base.h" -#include "HAL/Semaphore.hpp" #include "Controller.h" #include "LiveWindow/LiveWindow.h" +#include class PIDOutput; class PIDSource; @@ -69,6 +69,7 @@ private: 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 @@ -83,15 +84,15 @@ private: float m_result; float m_period; - MUTEX_ID m_semaphore; - PIDSource *m_pidInput; PIDOutput *m_pidOutput; - Notifier *m_controlLoop; + + pthread_t m_controlLoop; + pthread_mutex_t m_mutex; void Initialize(float p, float i, float d, float f, PIDSource *source, PIDOutput *output, float period = 0.05); - static void CallCalculate(void *controller); + static void *CallCalculate(void *controller); virtual ITable* GetTable(); virtual std::string GetSmartDashboardType(); diff --git a/wpilibc/wpilibC++/lib/PIDController.cpp b/wpilibc/wpilibC++/lib/PIDController.cpp index 79fe41e46a..f4688d2e9d 100644 --- a/wpilibc/wpilibC++/lib/PIDController.cpp +++ b/wpilibc/wpilibC++/lib/PIDController.cpp @@ -11,6 +11,7 @@ #include "PIDOutput.h" #include #include "HAL/cpp/Synchronized.hpp" +#include "Timer.h" static const char *kP = "p"; static const char *kI = "i"; @@ -32,8 +33,7 @@ static const char *kEnabled = "enabled"; */ PIDController::PIDController(float Kp, float Ki, float Kd, PIDSource *source, PIDOutput *output, - float period) : - m_semaphore (0) + float period) { Initialize(Kp, Ki, Kd, 0.0f, source, output, period); } @@ -50,22 +50,16 @@ PIDController::PIDController(float Kp, float Ki, float Kd, */ PIDController::PIDController(float Kp, float Ki, float Kd, float Kf, PIDSource *source, PIDOutput *output, - float period) : - m_semaphore (0) + float period) { Initialize(Kp, Ki, Kd, Kf, source, output, period); } - void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, PIDSource *source, PIDOutput *output, float period) { m_table = NULL; - - m_semaphore = initializeMutexNormal(); - - m_controlLoop = new Notifier(PIDController::CallCalculate, this); m_P = Kp; m_I = Ki; @@ -80,6 +74,7 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, m_continuous = false; m_enabled = false; + m_destruct = false; m_setpoint = 0; m_prevError = 0; @@ -91,8 +86,12 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, m_pidInput = source; m_pidOutput = output; m_period = period; - - m_controlLoop->StartPeriodic(m_period); + + pthread_mutexattr_t mutexattr; + pthread_mutexattr_settype(&mutexattr, PTHREAD_MUTEX_RECURSIVE); + pthread_mutex_init(&m_mutex, &mutexattr); + + pthread_create(&m_controlLoop, NULL, PIDController::CallCalculate, this); static int32_t instances = 0; instances++; @@ -106,22 +105,37 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf, */ PIDController::~PIDController() { - takeMutex(m_semaphore); - deleteMutex(m_semaphore); - delete m_controlLoop; + /* Let the calculation loop end before the std::thread object gets + destructed */ + pthread_mutex_lock(&m_mutex); + m_destruct = true; + pthread_mutex_unlock(&m_mutex); + + pthread_join(m_controlLoop, NULL); } /** * Call the Calculate method as a non-static method. This avoids having to prepend * all local variables in that method with the class pointer. This way the "this" * pointer will be set up and class variables can be called more easily. - * This method is static and called by the Notifier class. + * This method is static and called by pthreads. * @param controller the address of the PID controller object to use in the background loop */ -void PIDController::CallCalculate(void *controller) +void *PIDController::CallCalculate(void *data) { - PIDController *control = (PIDController*) controller; - control->Calculate(); + PIDController *controller = (PIDController*) data; + int destruct = 0; + + while(!destruct) { + controller->Calculate(); + + /* End the calculation loop when the PIDController gets destructed */ + pthread_mutex_lock(&controller->m_mutex); + destruct = controller->m_destruct; + pthread_mutex_unlock(&controller->m_mutex); + + Wait(controller->m_period); + } } /** @@ -133,67 +147,68 @@ void PIDController::Calculate() { bool enabled; PIDSource *pidInput; - - CRITICAL_REGION(m_semaphore) - { - if (m_pidInput == 0) return; - if (m_pidOutput == 0) return; - enabled = m_enabled; - pidInput = m_pidInput; - } - END_REGION; - - if (enabled) + + if(m_pidInput == 0) return; + if(m_pidOutput == 0) return; + + pthread_mutex_lock(&m_mutex); + enabled = m_enabled; + pidInput = m_pidInput; + pthread_mutex_unlock(&m_mutex); + + if(enabled) { + pthread_mutex_lock(&m_mutex); + float input = pidInput->PIDGet(); + float result; PIDOutput *pidOutput; + m_error = m_setpoint - input; + if (m_continuous) { - Synchronized sync(m_semaphore); - m_error = m_setpoint - input; - if (m_continuous) + if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) { - if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) + if (m_error > 0) { - if (m_error > 0) - { - m_error = m_error - m_maximumInput + m_minimumInput; - } - else - { - m_error = m_error + m_maximumInput - m_minimumInput; - } - } - } - - if(m_I != 0) - { - double potentialIGain = (m_totalError + m_error) * m_I; - if (potentialIGain < m_maximumOutput) - { - if (potentialIGain > m_minimumOutput) - m_totalError += m_error; - else - m_totalError = m_minimumOutput / m_I; + m_error = m_error - m_maximumInput + m_minimumInput; } else { - m_totalError = m_maximumOutput / m_I; + m_error = m_error + m_maximumInput - m_minimumInput; } } - - m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F; - m_prevError = m_error; - - if (m_result > m_maximumOutput) m_result = m_maximumOutput; - else if (m_result < m_minimumOutput) m_result = m_minimumOutput; - - pidOutput = m_pidOutput; - result = m_result; + } + + if(m_I != 0) + { + double potentialIGain = (m_totalError + m_error) * m_I; + if (potentialIGain < m_maximumOutput) + { + if (potentialIGain > m_minimumOutput) + m_totalError += m_error; + else + m_totalError = m_minimumOutput / m_I; + } + else + { + m_totalError = m_maximumOutput / m_I; + } } + m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F; + m_prevError = m_error; + + if (m_result > m_maximumOutput) m_result = m_maximumOutput; + else if (m_result < m_minimumOutput) m_result = m_minimumOutput; + + pidOutput = m_pidOutput; + result = m_result; + pidOutput->PIDWrite(result); + + pthread_mutex_unlock(&m_mutex); } } @@ -206,13 +221,11 @@ void PIDController::Calculate() */ void PIDController::SetPID(float p, float i, float d) { - CRITICAL_REGION(m_semaphore) - { - m_P = p; - m_I = i; - m_D = d; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_P = p; + m_I = i; + m_D = d; + pthread_mutex_unlock(&m_mutex); if (m_table != NULL) { m_table->PutNumber("p", m_P); @@ -231,14 +244,12 @@ void PIDController::SetPID(float p, float i, float d) */ void PIDController::SetPID(float p, float i, float d, float f) { - CRITICAL_REGION(m_semaphore) - { - m_P = p; - m_I = i; - m_D = d; - m_F = f; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_P = p; + m_I = i; + m_D = d; + m_F = f; + pthread_mutex_unlock(&m_mutex); if (m_table != NULL) { m_table->PutNumber("p", m_P); @@ -254,11 +265,13 @@ void PIDController::SetPID(float p, float i, float d, float f) */ float PIDController::GetP() { - CRITICAL_REGION(m_semaphore) - { - return m_P; - } - END_REGION; + float temp; + + pthread_mutex_lock(&m_mutex); + temp = m_P; + pthread_mutex_unlock(&m_mutex); + + return temp; } /** @@ -267,11 +280,13 @@ float PIDController::GetP() */ float PIDController::GetI() { - CRITICAL_REGION(m_semaphore) - { - return m_I; - } - END_REGION; + float temp; + + pthread_mutex_lock(&m_mutex); + temp = m_I; + pthread_mutex_unlock(&m_mutex); + + return temp; } /** @@ -280,11 +295,13 @@ float PIDController::GetI() */ float PIDController::GetD() { - CRITICAL_REGION(m_semaphore) - { - return m_D; - } - END_REGION; + float temp; + + pthread_mutex_lock(&m_mutex); + temp = m_D; + pthread_mutex_unlock(&m_mutex); + + return temp; } /** @@ -293,11 +310,13 @@ float PIDController::GetD() */ float PIDController::GetF() { - CRITICAL_REGION(m_semaphore) - { - return m_F; - } - END_REGION; + float temp; + + pthread_mutex_lock(&m_mutex); + temp = m_F; + pthread_mutex_unlock(&m_mutex); + + return temp; } /** @@ -307,13 +326,13 @@ float PIDController::GetF() */ float PIDController::Get() { - float result; - CRITICAL_REGION(m_semaphore) - { - result = m_result; - } - END_REGION; - return result; + float temp; + + pthread_mutex_lock(&m_mutex); + temp = m_result; + pthread_mutex_unlock(&m_mutex); + + return temp; } /** @@ -325,12 +344,9 @@ float PIDController::Get() */ void PIDController::SetContinuous(bool continuous) { - CRITICAL_REGION(m_semaphore) - { - m_continuous = continuous; - } - END_REGION; - + pthread_mutex_lock(&m_mutex); + m_continuous = continuous; + pthread_mutex_unlock(&m_mutex); } /** @@ -341,12 +357,10 @@ void PIDController::SetContinuous(bool continuous) */ void PIDController::SetInputRange(float minimumInput, float maximumInput) { - CRITICAL_REGION(m_semaphore) - { - m_minimumInput = minimumInput; - m_maximumInput = maximumInput; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_minimumInput = minimumInput; + m_maximumInput = maximumInput; + pthread_mutex_unlock(&m_mutex); SetSetpoint(m_setpoint); } @@ -359,12 +373,10 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput) */ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) { - CRITICAL_REGION(m_semaphore) - { - m_minimumOutput = minimumOutput; - m_maximumOutput = maximumOutput; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_minimumOutput = minimumOutput; + m_maximumOutput = maximumOutput; + pthread_mutex_unlock(&m_mutex); } /** @@ -373,23 +385,21 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) */ void PIDController::SetSetpoint(float setpoint) { - CRITICAL_REGION(m_semaphore) + pthread_mutex_lock(&m_mutex); + if (m_maximumInput > m_minimumInput) { - if (m_maximumInput > m_minimumInput) - { - if (setpoint > m_maximumInput) - m_setpoint = m_maximumInput; - else if (setpoint < m_minimumInput) - m_setpoint = m_minimumInput; - else - m_setpoint = setpoint; - } + if (setpoint > m_maximumInput) + m_setpoint = m_maximumInput; + else if (setpoint < m_minimumInput) + m_setpoint = m_minimumInput; else - { m_setpoint = setpoint; - } } - END_REGION; + else + { + m_setpoint = setpoint; + } + pthread_mutex_unlock(&m_mutex); if (m_table != NULL) { m_table->PutNumber("setpoint", m_setpoint); @@ -402,13 +412,13 @@ void PIDController::SetSetpoint(float setpoint) */ float PIDController::GetSetpoint() { - float setpoint; - CRITICAL_REGION(m_semaphore) - { - setpoint = m_setpoint; - } - END_REGION; - return setpoint; + float temp; + + pthread_mutex_lock(&m_mutex); + temp = m_setpoint; + pthread_mutex_unlock(&m_mutex); + + return temp; } /** @@ -418,11 +428,11 @@ float PIDController::GetSetpoint() float PIDController::GetError() { float error; - CRITICAL_REGION(m_semaphore) - { - error = m_setpoint - m_pidInput->PIDGet(); - } - END_REGION; + + pthread_mutex_lock(&m_mutex); + error = m_setpoint - m_pidInput->PIDGet(); + pthread_mutex_unlock(&m_mutex); + return error; } @@ -433,12 +443,10 @@ float PIDController::GetError() */ void PIDController::SetTolerance(float percent) { - CRITICAL_REGION(m_semaphore) - { - m_toleranceType = kPercentTolerance; - m_tolerance = percent; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_toleranceType = kPercentTolerance; + m_tolerance = percent; + pthread_mutex_unlock(&m_mutex); } /* @@ -448,12 +456,10 @@ void PIDController::SetTolerance(float percent) */ void PIDController::SetPercentTolerance(float percent) { - CRITICAL_REGION(m_semaphore) - { - m_toleranceType = kPercentTolerance; - m_tolerance = percent; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_toleranceType = kPercentTolerance; + m_tolerance = percent; + pthread_mutex_unlock(&m_mutex); } /* @@ -463,12 +469,10 @@ void PIDController::SetPercentTolerance(float percent) */ void PIDController::SetAbsoluteTolerance(float absTolerance) { - CRITICAL_REGION(m_semaphore) - { - m_toleranceType = kAbsoluteTolerance; - m_tolerance = absTolerance; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_toleranceType = kAbsoluteTolerance; + m_tolerance = absTolerance; + pthread_mutex_unlock(&m_mutex); } /* @@ -481,21 +485,21 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) bool PIDController::OnTarget() { bool temp; - CRITICAL_REGION(m_semaphore) - { - switch (m_toleranceType) { - case kPercentTolerance: - temp = fabs(GetError()) < (m_tolerance / 100 * (m_maximumInput - m_minimumInput)); - break; - case kAbsoluteTolerance: - temp = fabs(GetError()) < m_tolerance; - break; - //TODO: this case needs an error - case kNoTolerance: - temp = false; - } + + pthread_mutex_lock(&m_mutex); + switch (m_toleranceType) { + case kPercentTolerance: + temp = fabs(GetError()) < (m_tolerance / 100 * (m_maximumInput - m_minimumInput)); + break; + case kAbsoluteTolerance: + temp = fabs(GetError()) < m_tolerance; + break; + //TODO: this case needs an error + case kNoTolerance: + temp = false; } - END_REGION; + pthread_mutex_unlock(&m_mutex); + return temp; } @@ -504,11 +508,9 @@ bool PIDController::OnTarget() */ void PIDController::Enable() { - CRITICAL_REGION(m_semaphore) - { - m_enabled = true; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_enabled = true; + pthread_mutex_unlock(&m_mutex); if (m_table != NULL) { m_table->PutBoolean("enabled", true); @@ -520,12 +522,10 @@ void PIDController::Enable() */ void PIDController::Disable() { - CRITICAL_REGION(m_semaphore) - { - m_pidOutput->PIDWrite(0); - m_enabled = false; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_pidOutput->PIDWrite(0); + m_enabled = false; + pthread_mutex_unlock(&m_mutex); if (m_table != NULL) { m_table->PutBoolean("enabled", false); @@ -537,13 +537,13 @@ void PIDController::Disable() */ bool PIDController::IsEnabled() { - bool enabled; - CRITICAL_REGION(m_semaphore) - { - enabled = m_enabled; - } - END_REGION; - return enabled; + bool temp; + + pthread_mutex_lock(&m_mutex); + temp = m_enabled; + pthread_mutex_unlock(&m_mutex); + + return temp; } /** @@ -553,13 +553,11 @@ void PIDController::Reset() { Disable(); - CRITICAL_REGION(m_semaphore) - { - m_prevError = 0; - m_totalError = 0; - m_result = 0; - } - END_REGION; + pthread_mutex_lock(&m_mutex); + m_prevError = 0; + m_totalError = 0; + m_result = 0; + pthread_mutex_unlock(&m_mutex); } std::string PIDController::GetSmartDashboardType(){