diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
index 3a27781..c065b29 100644
--- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
+++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
@@ -122,8 +122,14 @@ protected:
             // Calculate the error in speed
             float error = targetSpeed - speed;
 
+            sumError += error; 
+
+            float errorDiff = error - prevError;
+
+            prevError = error;
+
             // Calculate the effort from the PID gains
-            int16_t effort = Kp * error;
+            int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff;
 
             // Set the effort for the motor
             SetEffort(effort);