diff --git a/car_fmw/src/Control.kt b/car_fmw/src/Control.kt
index 914d846646a..67fc9296eff 100644
--- a/car_fmw/src/Control.kt
+++ b/car_fmw/src/Control.kt
@@ -44,7 +44,7 @@ object Control {
             val time = times[i]
             val direction = directions[i]
 
-            Engine.drive(direction)
+            Engine.go(direction)
             Time.wait(time)
             Engine.stop()
 
diff --git a/car_fmw/src/Voyager.kt b/car_fmw/src/Voyager.kt
index b975d178690..1176cbb44d5 100644
--- a/car_fmw/src/Voyager.kt
+++ b/car_fmw/src/Voyager.kt
@@ -1,45 +1,18 @@
 
 object Voyager {
-    val VELOCITY_DRIVE: Double = 0.05 // centimeter in millisecond
-    val VEL0CITY_ROTATE: Double = 0.05 // degree in millisecond
-    val SEGMENT_SIZE: Int = 30 // centimeter
 
     val MAX_ANGLE: Int = 180 // degree
-    val ENGINE_DELAY: Int = 500 // milisecond
+    val SEGMENT_SIZE: Int = 30 // centimeter
 
     fun run() {
         while (true) {
-            var distance = getSmoothDistance(0)
+            var distance = Sonar.getSmoothDistance(0)
             while (distance == -1 || distance > 4 * SEGMENT_SIZE) {
-                drive(RouteType.FORWARD, SEGMENT_SIZE)
-                distance = getSmoothDistance(0)
+                Engine.drive(RouteType.FORWARD.id, SEGMENT_SIZE)
+                distance = Sonar.getSmoothDistance(0)
             }
 
-            rotate(45)
+            Engine.rotate(RouteType.LEFT.id, 45)
         }
     }
-
-    private fun rotate(degree: Int) {
-        val duration = (degree.toDouble() / VEL0CITY_ROTATE).toInt()
-        Engine.left()
-        Time.wait(duration)
-        smoothStop()
-    }
-
-    private fun drive(direction: RouteType, distance: Int) {
-        val duration = (distance.toDouble() / VELOCITY_DRIVE).toInt()
-        Engine.drive(direction.id)
-        Time.wait(duration)
-        smoothStop()
-    }
-
-    private fun getSmoothDistance(degree: Int): Int {
-        Sonar.getDistance(180)
-        return Sonar.getDistance(degree)
-    }
-
-    private fun smoothStop() {
-        Engine.stop()
-        Time.wait(ENGINE_DELAY)
-    }
 }
\ No newline at end of file
diff --git a/car_fmw/src/include/Engine.kt b/car_fmw/src/include/Engine.kt
index 4f5971fe860..ff8b57187da 100644
--- a/car_fmw/src/include/Engine.kt
+++ b/car_fmw/src/include/Engine.kt
@@ -14,6 +14,9 @@ enum class RouteType(val id: Int) {
 }
 
 object Engine {
+    val VELOCITY_DRIVE: Double = 0.05 // centimeter in millisecond
+    val VEL0CITY_ROTATE: Double = 0.05 // degree in millisecond
+
     fun init() {
         car_engine_init()
     }
@@ -38,7 +41,7 @@ object Engine {
         car_engine_turn_right()
     }
 
-    fun drive(direction: Int) {
+    fun go(direction: Int) {
         when (direction) {
             RouteType.FORWARD.id -> forward()
             RouteType.BACKWARD.id -> backward()
@@ -46,4 +49,16 @@ object Engine {
             RouteType.RIGHT.id -> right()
         }
     }
+
+    fun drive(direction: Int, distance: Int) {
+        go(direction)
+        Time.wait((distance.toDouble() / VELOCITY_DRIVE).toInt())
+        stop()
+    }
+
+    fun rotate(direction: Int, degree: Int) {
+        go(direction)
+        Time.wait((degree.toDouble() / VEL0CITY_ROTATE).toInt())
+        stop()
+    }
 }
\ No newline at end of file
diff --git a/car_fmw/src/include/Sonar.kt b/car_fmw/src/include/Sonar.kt
index 6b19de5a128..62f7e199cb4 100644
--- a/car_fmw/src/include/Sonar.kt
+++ b/car_fmw/src/include/Sonar.kt
@@ -1,4 +1,3 @@
-
 external fun car_sonar_init()
 external fun car_sonar_get_dist(degree: Byte): Short
 
@@ -8,4 +7,10 @@ object Sonar {
     }
 
     fun getDistance(degree: Int): Int = car_sonar_get_dist(degree.toByte()).toInt()
+
+    fun getSmoothDistance(degree: Int): Int {
+        Sonar.getDistance(180)
+        return Sonar.getDistance(degree)
+    }
+
 }
\ No newline at end of file
diff --git a/car_fmw/src/main.kt b/car_fmw/src/main.kt
index 65d6f22cd38..458d2b2815b 100644
--- a/car_fmw/src/main.kt
+++ b/car_fmw/src/main.kt
@@ -10,8 +10,8 @@ fun main() {
     Leds.set(Leds.GREEN, true)
 
     /* car task */
-//    Control.run()
+    Control.run()
 
     /* Voyager */
-    Voyager.run()
+//    Voyager.run()
 }