diff --git a/frtos/config/motor_config.h b/frtos/config/motor_config.h
index f3b7873..4219df3 100644
--- a/frtos/config/motor_config.h
+++ b/frtos/config/motor_config.h
@@ -24,9 +24,16 @@
 #define PWM_CLK_DIV                 250.f
 #define PWM_WRAP                    5000U
 
-#define PID_KP                      3.f
-#define PID_KI                      0.01 // 0.01f
-#define PID_KD                      0.05f // 0.05f
+/*
+ * ultimate gain Ku about 14, ultimate period Tu about 8 * 50 = 400ms
+ * Ku = 14, Tu = 400ms,
+ * Kp = 0.6 * Ku = 8.4
+ * Ki = Kp / Tu = 0.021
+ * Kd = Kp * Tu / 8 = 42
+ */
+#define PID_KP                      8.4f
+#define PID_KI                      0.021f // 0.005f
+#define PID_KD                      42.f // 0.05f
 
 #define MAX_SPEED                   4900U
 #define MIN_SPEED                   0U    // To be changed
@@ -46,6 +53,7 @@ typedef struct {
     SemaphoreHandle_t   sem;
     uint                slice_num;
     uint                pwm_channel;
+    float               distance;
 } motor_speed_t;
 
 #endif /* MOTOR_CONFIG_H */
\ No newline at end of file
diff --git a/frtos/motor/motor_init.h b/frtos/motor/motor_init.h
index 2ebdca3..970b12e 100644
--- a/frtos/motor/motor_init.h
+++ b/frtos/motor/motor_init.h
@@ -20,10 +20,12 @@
 #include "motor_config.h"
 
 motor_speed_t g_motor_speed_left  = { .pwm_level   = 2500u,
-                                      .pwm_channel = PWM_CHAN_A };
+                                      .pwm_channel = PWM_CHAN_A,
+                                      .distance = 0.0f,};
 
 motor_speed_t g_motor_speed_right = { .pwm_level   = 2500u,
-                                      .pwm_channel = PWM_CHAN_B };
+                                      .pwm_channel = PWM_CHAN_B,
+                                      .distance = 0.0f,};
 
 void
 motor_init(void)
diff --git a/frtos/motor/motor_pid.h b/frtos/motor/motor_pid.h
index d924819..cd4bf10 100644
--- a/frtos/motor/motor_pid.h
+++ b/frtos/motor/motor_pid.h
@@ -60,8 +60,8 @@ motor_pid_task(void *p_param)
             p_motor_speed->pwm_level = p_motor_speed->pwm_level + control_signal;
         }
 
-        printf("control signal: %f\n", control_signal);
-        printf("new pwm: %hu\n\n", p_motor_speed->pwm_level);
+        // printf("control signal: %f\n", control_signal);
+        // printf("new pwm: %hu\n\n", p_motor_speed->pwm_level);
 
         pwm_set_chan_level(p_motor_speed->slice_num,
                            p_motor_speed->pwm_channel,
diff --git a/frtos/motor/motor_speed.h b/frtos/motor/motor_speed.h
index 2b49b82..525601f 100644
--- a/frtos/motor/motor_speed.h
+++ b/frtos/motor/motor_speed.h
@@ -66,12 +66,16 @@ monitor_wheel_speed_task(void *pvParameters)
                                                        (elapsed_time /
                                                         1000000.f));
 
-            printf("speed: %f cm/s\n", p_motor_speed->current_speed_cms);
+            p_motor_speed->distance += 1.02101761242f;
+
+            // printf("speed: %f cm/s\n", p_motor_speed->current_speed_cms);
         }
         else
         {
             p_motor_speed->current_speed_cms = 0.f;
-            printf("stopped\n");
+            // printf("stopped\n");
         }
+
+        // printf("distance: %f cm\n", p_motor_speed->distance);
     }
 }
\ No newline at end of file
diff --git a/frtos/motor/motor_test.c b/frtos/motor/motor_test.c
index 7370d91..0e9268b 100644
--- a/frtos/motor/motor_test.c
+++ b/frtos/motor/motor_test.c
@@ -11,13 +11,13 @@ test_speed_change_task(void *p_param)
 {
     for (;;)
     {
-        g_motor_speed_left.target_speed_cms  = 50.0f;
-        g_motor_speed_right.target_speed_cms = 50.0f;
+        g_motor_speed_left.target_speed_cms  = 15.0f;
+        g_motor_speed_right.target_speed_cms = 15.0f;
         vTaskDelay(pdMS_TO_TICKS(5000));
 
-        g_motor_speed_left.target_speed_cms  = 20.0f;
-        g_motor_speed_right.target_speed_cms = 20.0f;
-        vTaskDelay(pdMS_TO_TICKS(5000));
+//        g_motor_speed_left.target_speed_cms  = 20.0f;
+//        g_motor_speed_right.target_speed_cms = 20.0f;
+//        vTaskDelay(pdMS_TO_TICKS(5000));
 
         g_motor_speed_left.target_speed_cms  = 0.0f;
         g_motor_speed_right.target_speed_cms = 0.0f;