car_fmw: Voyager velocity

car_fmw/src/Voyager.kt

@@ -1,7 +1,7 @@
 
 object Voyager {
-    val VELOCITY_DRIVE: Double = 0.03 // centimeter in millisecond
-    val VEL0CITY_ROUTATE: Double = 0.015 // degree in millisecond
+    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
@@ -9,20 +9,21 @@ object Voyager {
 
     fun run() {
         while (true) {
-            var distance = Sonar.getDistance(0)
-            while (distance == -1 || distance > SEGMENT_SIZE) {
+            var distance = getSmoothDistance(0)
+            while (distance == -1 || distance > 4 * SEGMENT_SIZE) {
                 drive(RouteType.FORWARD, SEGMENT_SIZE)
-                distance = Sonar.getDistance(0)
+                distance = getSmoothDistance(0)
             }
 
-            rotate(Random.getInt() % MAX_ANGLE)
+            rotate(45)
         }
     }
 
     private fun rotate(degree: Int) {
-        val duration = (degree.toDouble() / VELOCITY_DRIVE).toInt()
+        val duration = (degree.toDouble() / VEL0CITY_ROTATE).toInt()
         Engine.left()
         Time.wait(duration)
+        smoothStop()
     }
 
     private fun drive(direction: RouteType, distance: Int) {
@@ -32,6 +33,11 @@ object Voyager {
         smoothStop()
     }
 
+    private fun getSmoothDistance(degree: Int): Int {
+        Sonar.getDistance(180)
+        return Sonar.getDistance(degree)
+    }
+
     private fun smoothStop() {
         Engine.stop()
         Time.wait(ENGINE_DELAY)

car_hw/src/car_engine.c

@@ -56,7 +56,7 @@ void car_engine_backward(void)
     GPIO_SetBits(CAR_ENGINE_GPIO_PORT, CAR_ENGINE_RWHEEL_BKWD_PIN);
 }
 
-void car_engine_turn_left(void)
+void car_engine_turn_right(void)
 {
     car_engine_stop();
     GPIO_SetBits(CAR_ENGINE_GPIO_PORT, CAR_ENGINE_ENABLE_PIN);
@@ -64,7 +64,7 @@ void car_engine_turn_left(void)
     GPIO_SetBits(CAR_ENGINE_GPIO_PORT, CAR_ENGINE_RWHEEL_FWD_PIN);
 }
 
-void car_engine_turn_right(void)
+void car_engine_turn_left(void)
 {
     car_engine_stop();
     GPIO_SetBits(CAR_ENGINE_GPIO_PORT, CAR_ENGINE_ENABLE_PIN);