diff --git a/src/control/PID.cpp b/src/control/PID.cpp
index b2843aa..2ef540d 100644
--- a/src/control/PID.cpp
+++ b/src/control/PID.cpp
@@ -87,19 +87,32 @@ float controllerPI::run(float dT, float input, float setPoint)
 {
     float error(setPoint-input);
 
-    float pTerm(m_kP*error);
+    float pTerm(m_kPScale * m_kP * error);
 
     // Accumulate to the integrator
-    float iTerm(constrain(m_integrator + m_kI*error*dT, m_iTermSaturation));
+    m_integrator += error * dT;
+    m_integrator = constrain(m_integrator, m_iTermSaturation);
 
-    // Store
-    m_integrator = iTerm;
+    float iTerm = m_integrator * m_kIScale * m_kI;
 
     float output(constrain(pTerm + iTerm, m_saturation));
 
     return output;
 }
 
+void controllerPI::setGainScaling(float kPScale, float kIScale)
+{
+    static float scaleFactor(1.0f / 1000.0f);
+    
+    m_kPScale = (1000.0f + kPScale) * scaleFactor;
+    m_kIScale = (1000.0f + kIScale) * scaleFactor;
+}
+
+void controllerPI::flushIntegral()
+{
+    m_integrator = 0;
+}
+
 /*----------------------------------- PID Controller ---------------------------------*/
 
 controllerPID::controllerPID(float kP, float kI, float kD, float saturation, float iTermSaturation)
@@ -113,7 +126,7 @@ controllerPID::controllerPID(float kP, float kI, float kD, float saturation, flo
 , m_lastError(0.0f)
 , m_iTermSaturation(iTermSaturation)
 , m_integrator(0.0f)
-, m_pt1FilterApply4(100.0f)
+, m_pt1FilterApply4(20.0f)
 {
 }
 
@@ -124,13 +137,10 @@ float controllerPID::run(float dT, float input, float setPoint)
     float pTerm(m_kPScale * m_kP * error);
 
     // Accumulate to the integrator
-    float iTerm(
-        constrain(
-            m_integrator + (m_kIScale * m_kI * error * dT), 
-            m_iTermSaturation));
+    m_integrator += error * dT;
+    m_integrator = constrain(m_integrator, m_iTermSaturation);
 
-    // Store
-    m_integrator = iTerm;
+    float iTerm = m_integrator * m_kIScale * m_kI;
 
     float dTerm((dT != 0.0f) ? m_kDScale * m_kD * ((error-m_lastError)/dT) : 0.0f);
 
@@ -153,4 +163,64 @@ void controllerPID::setGainScaling(float kPScale, float kIScale, float kDScale)
     m_kDScale = (1000.0f + kDScale) * scaleFactor;
 }
 
+void controllerPID::flushIntegral()
+{
+    m_integrator = 0;
+}
+
+/*----------------------------------- PID Controller ---------------------------------*/
+
+controllerPID2::controllerPID2(float kP, float kI, float kD, float saturation, float iTermSaturation)
+: m_kP(kP)
+, m_kI(kI)
+, m_kD(kD)
+, m_kPScale(0.0f)
+, m_kIScale(0.0f)
+, m_kDScale(0.0f)
+, m_saturation(saturation)
+, m_iTermSaturation(iTermSaturation)
+, m_integrator(0.0f)
+, m_inputTMinus1(0.0f)
+, m_inputTMinus2(0.0f)
+, m_setPointTMinus1(0.0f)
+{
+}
+
+float controllerPID2::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kPScale * m_kP * error);
+
+    // Accumulate to the integrator
+    m_integrator += error * dT;
+    m_integrator = constrain(m_integrator, m_iTermSaturation);
+
+    float iTerm = m_integrator * m_kIScale * m_kI;
+
+    float dTerm = + (m_kD*m_kDScale*((setPoint - m_setPointTMinus1) - (input - m_inputTMinus2)))/dT;
+    
+    m_inputTMinus2 = m_inputTMinus1;
+    m_inputTMinus1 = input;
+    m_setPointTMinus1 = setPoint;
+
+    float output(constrain(pTerm + iTerm + dTerm, m_saturation));
+
+    return output;
+}
+
+void controllerPID2::setGainScaling(float kPScale, float kIScale, float kDScale)
+{
+    static float scaleFactor(1.0f / 1000.0f);
+    
+    m_kPScale = (1000.0f + kPScale) * scaleFactor;
+    m_kIScale = (1000.0f + kIScale) * scaleFactor;
+    m_kDScale = (1000.0f + kDScale) * scaleFactor;
+}
+
+void controllerPID2::flushIntegral()
+{
+    m_integrator = 0;
+}
+
 }
\ No newline at end of file
diff --git a/src/control/PID.h b/src/control/PID.h
index 80cefbe..e623073 100644
--- a/src/control/PID.h
+++ b/src/control/PID.h
@@ -52,9 +52,13 @@ public:
 
     float run(float dT, float input, float setPoint);
 
+    void setGainScaling(float kPScale, float kIScale);
+
+    void flushIntegral();
+
 private:
-    float m_kP;
-    float m_kI;
+    float m_kP, m_kI;
+    float m_kPScale, m_kIScale;
     float m_saturation;
     float m_iTermSaturation;
     float m_integrator;
@@ -71,6 +75,8 @@ public:
     //! from [0-2].
     void setGainScaling(float kPScale, float kIScale, float kDScale);
 
+    void flushIntegral();
+
 private:
     float m_kP, m_kI, m_kD;
     float m_kPScale, m_kIScale, m_kDScale;
@@ -81,4 +87,28 @@ private:
     pt1FilterApply4 m_pt1FilterApply4;
 };
 
+class controllerPID2
+{
+public:
+    controllerPID2(float kP, float kI, float kD, float saturation, float iTermSaturation);
+
+    float run(float dT, float input, float setPoint);
+
+    //! Input values in range [-1000, 1000] will result in scales 
+    //! from [0-2].
+    void setGainScaling(float kPScale, float kIScale, float kDScale);
+
+    void flushIntegral();
+
+private:
+    float m_kP, m_kI, m_kD;
+    float m_kPScale, m_kIScale, m_kDScale;
+    float m_saturation;
+    float m_iTermSaturation;
+    float m_integrator;
+    float m_inputTMinus1;
+    float m_inputTMinus2;
+    float m_setPointTMinus1;
+};
+
 }
\ No newline at end of file