diff --git a/tfrog-motordriver/communication.c b/tfrog-motordriver/communication.c
index d40a7ce..ca87e88 100644
--- a/tfrog-motordriver/communication.c
+++ b/tfrog-motordriver/communication.c
@@ -1524,10 +1524,12 @@ int command_analyze(unsigned char* data, int len)
     case PARAM_w_ref:
       i.integer *= 16;
     case PARAM_w_ref_highprec:
-      if (motor[imotor].ref.vel != i.integer)
-        motor[imotor].ref.vel_changed = 1;
+    {
+      const char changed = (motor[imotor].ref.vel != i.integer);
       motor[imotor].ref.vel = i.integer;
+      motor[imotor].ref.vel_changed = changed;
       break;
+    }
     case PARAM_p_toq_offset:
       motor[imotor].ref.torque_offset = i.integer;
       break;
diff --git a/tfrog-motordriver/controlVelocity.c b/tfrog-motordriver/controlVelocity.c
index 993e98a..bd0a9ca 100644
--- a/tfrog-motordriver/controlVelocity.c
+++ b/tfrog-motordriver/controlVelocity.c
@@ -109,7 +109,7 @@ void ISR_VelocityControl()
           motor[i].ref.vel_diff = 0;
         }
 
-        motor[i].error = motor[i].ref.vel_buf - motor[i].vel;
+        motor[i].error = motor[i].ref.vel - motor[i].vel;
         motor[i].error_integ += motor[i].error;
         if (motor[i].error_integ > motor_param[i].integ_max)
         {