From 1370952a955508cf33175fd561ffbeab13378444 Mon Sep 17 00:00:00 2001
From: e5l <leonid.stashevsky@gmail.com>
Date: Thu, 25 Aug 2016 13:26:10 +0300
Subject: [PATCH] car_fmw: Voyager velocity

---
 car_fmw/src/Voyager.kt  | 20 +++++++++++++-------
 car_hw/src/car_engine.c |  4 ++--
 2 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/car_fmw/src/Voyager.kt b/car_fmw/src/Voyager.kt
index 779991f189e..b975d178690 100644
--- a/car_fmw/src/Voyager.kt
+++ b/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)
diff --git a/car_hw/src/car_engine.c b/car_hw/src/car_engine.c
index 0b5e3e95f4e..8f5d5368924 100644
--- a/car_hw/src/car_engine.c
+++ b/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);