Made PIDController use pthreads in C++

Change-Id: I3d4d1aa912bf56faa1d5e150732da5e7b551077d

wpilibc/wpilibC++/lib/PIDController.cpp

@@ -11,6 +11,7 @@
 #include "PIDOutput.h"
 #include <math.h>
 #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(){