diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp
index eeca269..0b81873 100644
--- a/controller_teensy/src/main.cpp
+++ b/controller_teensy/src/main.cpp
@@ -315,7 +315,7 @@ void loop() {
     sendCMD();    
 
     //Update speed and trip
-    float _meanRPM=FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas;
+    float _meanRPM=(FeedbackFront.speedL_meas-FeedbackFront.speedR_meas+FeedbackRear.speedL_meas-FeedbackRear.speedR_meas)/4.0;
     meanSpeedms=_meanRPM*wheelcircumference/60.0;   // Units: 1/min * m / 60s
     trip+=abs(meanSpeedms)* (SENDPERIOD/1000.0);