diff --git a/firmware/controllers/math/pid.cpp b/firmware/controllers/math/pid.cpp
index a0cb789a74..37b7784408 100644
--- a/firmware/controllers/math/pid.cpp
+++ b/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) {
-	return 0;
+float Pid::getValue(float target, float input, float dTime) {
+	float error = target - input;
+
+	float pTerm = pFactor * error;
+	integration += iFactor * dTime * error;
+	float dTerm = dFactor / dTime * (error - prevError);
+
+	prevError = error;
+	return pTerm + integration + dTerm;
 }
 
diff --git a/firmware/controllers/math/pid.h b/firmware/controllers/math/pid.h
index 1044aeb931..528643dc60 100644
--- a/firmware/controllers/math/pid.h
+++ b/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_ */