From e586c828f3ffec41c31a23522fb1c520c1a038e8 Mon Sep 17 00:00:00 2001
From: Andrea Nistico <andrea.nistic@gmail.com>
Date: Tue, 23 May 2017 16:24:52 +0200
Subject: [PATCH 1/3] better derivative handling, if needed

This commit adds the possibility to use the derivative of the setpoint as well for the differential part.
---
 MiniPID.cpp | 20 ++++++++++++++++++--
 MiniPID.h   |  5 +++++
 2 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/MiniPID.cpp b/MiniPID.cpp
index 18cc516..adfb08f 100644
--- a/MiniPID.cpp
+++ b/MiniPID.cpp
@@ -37,6 +37,7 @@ void MiniPID::init(){
 	lastActual=0;
 	firstRun=true;
 	reversed=false;
+	useDt=false;
 	outputRampRate=0;
 	lastOutput=0;
 	outputFilter=0;
@@ -163,6 +164,9 @@ void MiniPID::setOutputLimits(double minimum,double maximum){
 void MiniPID::setDirection(bool reversed){
 	this->reversed=reversed;
 }
+void MiniPID::useDeltaTime(bool useDt){
+	this->useDt=useDt;
+}
 
 //**********************************
 //Primary operating functions
@@ -218,8 +222,14 @@ double MiniPID::getOutput(double actual, double setpoint){
 	//Note, this->is negative. this->actually "slows" the system if it's doing
 	//the correct thing, and small values helps prevent output spikes and overshoot 
 
-	Doutput= -D*(actual-lastActual);
-	lastActual=actual;
+	if(useDt && (dt > 0)){
+		Doutput= D*((setpoint-lastSetpoint) - (actual-lastActual)) / dt;
+		lastActual=actual;
+		lastSetpoint=setpoint;
+	} else{
+		Doutput= -D*(actual-lastActual);
+		lastActual=actual;
+	}
 
 
 
@@ -323,6 +333,12 @@ void MiniPID::setOutputFilter(double strength){
 		outputFilter=strength;
 	}
 }
+/**
+ * Sets the delta time between each iteration.
+ */
+void MiniPID::setDt(double dt){
+	this->dt=dt;
+}
 
 //**************************************
 // Helper functions
diff --git a/MiniPID.h b/MiniPID.h
index 55ed3ab..cecc3ef 100644
--- a/MiniPID.h
+++ b/MiniPID.h
@@ -20,6 +20,8 @@ class MiniPID{
 	void setOutputRampRate(double);
 	void setSetpointRange(double);
 	void setOutputFilter(double);
+	void setDt(double);
+	void useDeltaTime(bool);
 	double getOutput();
 	double getOutput(double);
 	double getOutput(double, double);
@@ -33,6 +35,7 @@ class MiniPID{
 	double I;
 	double D;
 	double F;
+	double dt;
 
 	double maxIOutput;
 	double maxError;
@@ -42,11 +45,13 @@ class MiniPID{
 	double minOutput;
 
 	double setpoint;
+	double lastSetpoint;
 
 	double lastActual;
 
 	bool firstRun;
 	bool reversed;
+	bool useDt;
 
 	double outputRampRate;
 	double lastOutput;

From 392f1c52ee5001114679fe60c06ea8445ca3a374 Mon Sep 17 00:00:00 2001
From: Andrea Nistico <andrea.nistic@gmail.com>
Date: Tue, 23 May 2017 16:28:34 +0200
Subject: [PATCH 2/3] init dt to zero

---
 MiniPID.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/MiniPID.cpp b/MiniPID.cpp
index adfb08f..810783b 100644
--- a/MiniPID.cpp
+++ b/MiniPID.cpp
@@ -42,6 +42,7 @@ void MiniPID::init(){
 	lastOutput=0;
 	outputFilter=0;
 	setpointRange=0;
+	dt=0;
 }
 
 //**********************************

From b71f3f6b77e6f6d749f76e2a7c734e12090e2b8e Mon Sep 17 00:00:00 2001
From: Andrea Nistico <andrea.nistic@gmail.com>
Date: Tue, 23 May 2017 16:32:25 +0200
Subject: [PATCH 3/3] add description and lastSetpoint init

---
 MiniPID.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/MiniPID.cpp b/MiniPID.cpp
index 810783b..f346053 100644
--- a/MiniPID.cpp
+++ b/MiniPID.cpp
@@ -35,6 +35,7 @@ void MiniPID::init(){
 	minOutput=0;
 	setpoint=0;
 	lastActual=0;
+	lastSetpoint=0;
 	firstRun=true;
 	reversed=false;
 	useDt=false;
@@ -165,6 +166,9 @@ void MiniPID::setOutputLimits(double minimum,double maximum){
 void MiniPID::setDirection(bool reversed){
 	this->reversed=reversed;
 }
+/** Set the desired differential part calculation
+ * @param useDt Set true to calculate time derivative
+ */
 void MiniPID::useDeltaTime(bool useDt){
 	this->useDt=useDt;
 }
@@ -214,6 +218,7 @@ double MiniPID::getOutput(double actual, double setpoint){
 	//For last output, we can assume it's the current time-independent outputs. 
 	if(firstRun){
 		lastActual=actual;
+		lastSetpoint=setpoint;
 		lastOutput=Poutput+Foutput;
 		firstRun=false;
 	}