From e15137205b836173410df901f1d882163706d1ca Mon Sep 17 00:00:00 2001
From: zhivko <klemen.zivkovic@gmail.com>
Date: Wed, 8 Aug 2018 19:31:04 +0200
Subject: [PATCH 1/2] Allow synchronisation of two pids

---
 MiniPID.cpp | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/MiniPID.cpp b/MiniPID.cpp
index 18cc516..9323f36 100644
--- a/MiniPID.cpp
+++ b/MiniPID.cpp
@@ -230,11 +230,22 @@ double MiniPID::getOutput(double actual, double setpoint){
 	Ioutput=I*errorSum;
 	if(maxIOutput!=0){
 		Ioutput=clamp(Ioutput,-maxIOutput,maxIOutput); 
-	}	
+	}
 
 	//And, finally, we can just add the terms up
 	output=Foutput + Poutput + Ioutput + Doutput;
 
+	// To synchronize two motors we are using this->positionDiff
+	// if this->positionDiff is positive this means we need to reduce output and vice versa
+	// if output is negative we need to add
+	//output = output + (sgn(output) * -1.0 * this->positionDiff * maxOutput*0.05);
+	
+	float posOutputFilter = 0.35;
+	double POSOutput = clamp(this->positionDiff, -5.0, 5.0) * maxOutput*0.01;
+	double POSOutputFiltered=POSOutput*posOutputFilter+POSOutput*(1-posOutputFilter);
+	output = output - POSOutputFiltered;
+
+
 	//Figure out what we're doing with the error.
 	if(minOutput!=maxOutput && !bounded(output, minOutput,maxOutput) ){
 		errorSum=error; 
@@ -370,3 +381,8 @@ void MiniPID::checkSigns(){
 		if(F<0) F*=-1;
 	}
 }
+
+void MiniPID::setPositionDiff(double _positionDiff)
+{
+	this->positionDiff = _positionDiff;
+}

From c5b7c17078dc98f5290a1f7291a14015a21df3dc Mon Sep 17 00:00:00 2001
From: zhivko <klemen.zivkovic@gmail.com>
Date: Wed, 8 Aug 2018 19:31:55 +0200
Subject: [PATCH 2/2] Update MiniPID.h

---
 MiniPID.h | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/MiniPID.h b/MiniPID.h
index 55ed3ab..7bc8230 100644
--- a/MiniPID.h
+++ b/MiniPID.h
@@ -1,6 +1,10 @@
 #ifndef MINIPID_H
 #define MINIPID_H
 
+template <typename T> int sgn(T val) {
+    return (T(0) < val) - (val < T(0));
+}
+
 class MiniPID{
 public:
 	MiniPID(double, double, double);
@@ -20,10 +24,12 @@ class MiniPID{
 	void setOutputRampRate(double);
 	void setSetpointRange(double);
 	void setOutputFilter(double);
+	void setPositionDiff(double);
 	double getOutput();
 	double getOutput(double);
 	double getOutput(double, double);
 
+
 private:
 	double clamp(double, double, double);
 	bool bounded(double, double, double);
@@ -38,6 +44,8 @@ class MiniPID{
 	double maxError;
 	double errorSum;
 
+	double positionDiff;
+
 	double maxOutput; 
 	double minOutput;