car_fmw: sonar missing angle compensation

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++
         }
 

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() {

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() {

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)
+    }
 }