From 1ae02c367eae7020234b4d87ad7cc08035e93a2a Mon Sep 17 00:00:00 2001
From: e5l <leonid.stashevsky@gmail.com>
Date: Fri, 26 Aug 2016 12:08:16 +0300
Subject: [PATCH] car_fmw: sonar missing angle compensation

---
 car_fmw/src/Control.kt        |  8 +-------
 car_fmw/src/Voyager.kt        |  3 +--
 car_fmw/src/include/Engine.kt |  3 ++-
 car_fmw/src/include/Sonar.kt  | 36 ++++++++++++++++++++++++++++++-----
 4 files changed, 35 insertions(+), 15 deletions(-)

diff --git a/car_fmw/src/Control.kt b/car_fmw/src/Control.kt
index 481058737ec..feb78d7030c 100644
--- a/car_fmw/src/Control.kt
+++ b/car_fmw/src/Control.kt
@@ -11,18 +11,12 @@ object Control {
 
     private fun executeCommand() {
         val task = Reader.readTask()
-        Leds.blink()
-        Time.wait(BLINK_DURATION)
-
         when (task.type.id) {
             TaskRequest.Type.DEBUG.id -> debugTask()
             TaskRequest.Type.ROUTE.id -> routeTask()
             TaskRequest.Type.ROUTE_METRIC.id -> routeMetricTask()
             TaskRequest.Type.SONAR.id -> sonarTask()
         }
-
-        Leds.blink()
-        Time.wait(BLINK_DURATION)
     }
 
     private fun debugTask() {
@@ -79,7 +73,7 @@ object Control {
 
         var i = 0
         while (i < size) {
-            distances[i] = Sonar.getDistance(request.angles[i])
+            distances[i] = Sonar.getSmoothDistance(request.angles[i])
             i++
         }
 
diff --git a/car_fmw/src/Voyager.kt b/car_fmw/src/Voyager.kt
index 5efad5500f5..fa003cbb656 100644
--- a/car_fmw/src/Voyager.kt
+++ b/car_fmw/src/Voyager.kt
@@ -1,7 +1,6 @@
 
 object Voyager {
-
-    val MAX_ANGLE: Int = 180 // degree
+    val MAX_ANGLE: Int = 180 // angle
     val SEGMENT_SIZE: Int = 30 // centimeter
 
     fun run() {
diff --git a/car_fmw/src/include/Engine.kt b/car_fmw/src/include/Engine.kt
index 6d99236cce0..e3a57aedebb 100644
--- a/car_fmw/src/include/Engine.kt
+++ b/car_fmw/src/include/Engine.kt
@@ -15,7 +15,7 @@ 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
+    val VEL0CITY_ROTATE: Double = 0.05 // angle in millisecond
 
     fun init() {
         car_engine_init()
@@ -23,6 +23,7 @@ object Engine {
 
     fun stop() {
         car_engine_stop()
+        Sonar.calibrate()
     }
 
     fun forward() {
diff --git a/car_fmw/src/include/Sonar.kt b/car_fmw/src/include/Sonar.kt
index 62f7e199cb4..77e3df514cc 100644
--- a/car_fmw/src/include/Sonar.kt
+++ b/car_fmw/src/include/Sonar.kt
@@ -2,15 +2,41 @@ external fun car_sonar_init()
 external fun car_sonar_get_dist(degree: Byte): Short
 
 object Sonar {
+    private var ATTEMPTS_COUNT = 5
+    private var DEVIATION = 10
+    private var SCAN_STEP = 1
+
     fun init() {
         car_sonar_init()
     }
 
-    fun getDistance(degree: Int): Int = car_sonar_get_dist(degree.toByte()).toInt()
-
-    fun getSmoothDistance(degree: Int): Int {
-        Sonar.getDistance(180)
-        return Sonar.getDistance(degree)
+    fun getDistance(angle: Int): Int {
+        return car_sonar_get_dist(angle.toByte()).toInt()
     }
 
+    fun getSmoothDistance(angle: Int): Int {
+        val distance = getDistance(angle)
+        if (distance != -1) {
+            return distance
+        }
+
+        val start = if (angle - DEVIATION < 0) 0 else angle - DEVIATION
+        return getOneOfRange(start, angle + DEVIATION, SCAN_STEP)
+    }
+
+    fun getOneOfRange(start: Int, stop: Int, step: Int): Int {
+        var i = start
+        var distance = getDistance(start)
+        while (i <= stop && distance == -1) {
+            distance = getDistance(i)
+
+            i += step
+        }
+
+        return distance
+    }
+
+    fun calibrate() {
+        getDistance(180)
+    }
 }
\ No newline at end of file