auto-sync

firmware/controllers/math/pid.cpp

@@ -13,9 +13,19 @@ Pid::Pid(float pFactor, float iFactor, float dFactor) {
 	this->pFactor = pFactor;
 	this->iFactor = iFactor;
 	this->dFactor = dFactor;
+
+	integration = 0;
+	prevError = 0;
 }
 
-float Pid::getValue(float input) {
+float Pid::getValue(float target, float input, float dTime) {
-	return 0;
+	float error = target - input;
+
+	float pTerm = pFactor * error;
+	integration += iFactor * dTime * error;
+	float dTerm = dFactor / dTime * (error - prevError);
+
+	prevError = error;
+	return pTerm + integration + dTerm;
 }
 

firmware/controllers/math/pid.h

@@ -12,13 +12,16 @@ class Pid {
 
 public:
 	Pid(float pFactor, float iFactor, float dFactor);
-	float getValue(float input);
+	float getValue(float target, float input, float dTime);
 
 private:
 	float pFactor;
 	float iFactor;
 	float dFactor;
 
+	float integration;
+	float prevError;
+
 };
 
 #endif /* PID_H_ */