Compute derivative

MiniPID.cpp

@@ -35,12 +35,15 @@ void MiniPID::init(){
 	minOutput=0;
 	setpoint=0;
 	lastActual=0;
+	lastSetpoint=0;
 	firstRun=true;
 	reversed=false;
+	useDt=false;
 	outputRampRate=0;
 	lastOutput=0;
 	outputFilter=0;
 	setpointRange=0;
+	dt=0;
 }
 
 //**********************************
@@ -163,6 +166,12 @@ 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;
+}
 
 //**********************************
 //Primary operating functions
@@ -209,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;
 	}
@@ -218,8 +228,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 +339,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

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;