diff --git a/stm-firmware/pid-controller.c b/stm-firmware/pid-controller.c
index 83e7f77..dbae742 100644
--- a/stm-firmware/pid-controller.c
+++ b/stm-firmware/pid-controller.c
@@ -85,7 +85,7 @@ float pid_sample(struct pid_controller *pid, float deviation)
 	}
 
 	/* Calculate derivative part */
-	pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) -  pid->k_inv_deriv_t * pid->derivate;
+	pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) + pid->k_inv_deriv_t * pid->derivate;
 
 	output += pid->derivate;
 	output += pid->integral;