car_srv: filter far walls, minor fixes; car_fmw: fix scanning algorithm

car_fmw/src/Control.kt

@@ -87,7 +87,8 @@ object Control {
         val data = IntArray(attempts)
         var i = 0
         while (i < attempts) {
-            data[i] = Sonar.getSmoothDistance(angle, windowSize)
+            data[i] = Sonar.getSmoothDistance(angle, windowSize, smoothing)
+
             if (smoothing.id == SonarRequest.Smoothing.NONE.id && data[i] != -1) {
                 return data[i]
             }
@@ -95,8 +96,8 @@ object Control {
         }
 
         return when (smoothing.id) {
-            SonarRequest.Smoothing.MEAN.id -> data.mean()
+            SonarRequest.Smoothing.MEAN.id -> data.filter(::positive).mean()
-            SonarRequest.Smoothing.MEDIAN.id -> data.median()
+            SonarRequest.Smoothing.MEDIAN.id -> data.filter(::positive).median()
             else -> -1
         }
     }

car_fmw/src/Voyager.kt

@@ -6,10 +6,10 @@ object Voyager {
 
     fun run() {
         while (true) {
-            var distance = Sonar.getSmoothDistance(0, DEFAULT_WINDOW)
+            var distance = Sonar.getSmoothDistance(0, DEFAULT_WINDOW, SonarRequest.Smoothing.NONE)
             while (distance == -1 || distance > 4 * SEGMENT_SIZE) {
                 Engine.drive(RouteType.FORWARD.id, SEGMENT_SIZE)
-                distance = Sonar.getSmoothDistance(0, DEFAULT_WINDOW)
+                distance = Sonar.getSmoothDistance(0, DEFAULT_WINDOW, SonarRequest.Smoothing.NONE)
             }
 
             Engine.drive(RouteType.LEFT.id, 45)

car_fmw/src/include/Sonar.kt

@@ -2,8 +2,6 @@ external fun car_sonar_init()
 external fun car_sonar_get_dist(degree: Byte): Short
 
 object Sonar {
-    private var SCAN_STEP = 1
-
     fun init() {
         car_sonar_init()
     }
@@ -11,32 +9,67 @@ object Sonar {
     fun getDistance(angle: Int): Int =
             car_sonar_get_dist(angle.toByte()).toInt()
 
-    fun getSmoothDistance(angle: Int, windowSize: Int): Int {
+    fun getSmoothDistance(angle: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): Int {
         val distance = getDistance(angle)
         if (distance != -1 || windowSize == 0) {
             return distance
         }
 
         val start = if (angle - windowSize < 0) 0 else angle - windowSize
-        return getOneOfRange(start, angle + windowSize, SCAN_STEP)
+        val stop = angle + windowSize
+        when (smoothing.id) {
+            SonarRequest.Smoothing.NONE.id -> {
+                return getOneOfRange(start, stop)
+            }
+            SonarRequest.Smoothing.MEDIAN.id -> {
+                val data = getRange(start, stop).filter(::positive)
+                if (data.size == 0) {
+                    return -1
+                }
+
+                return data.median()
+            }
+            SonarRequest.Smoothing.MEAN.id -> {
+                val data = getRange(start, stop).filter(::positive)
+                if (data.size == 0) {
+                    return -1
+                }
+
+                return data.mean()
+            }
+        }
+
+        return -1
     }
 
-    fun getOneOfRange(start: Int, stop: Int, step: Int): Int {
+    private fun getOneOfRange(start: Int, stop: Int): Int {
         var i = start
         var distance = getDistance(start)
         while (i <= stop && distance == -1) {
             distance = getDistance(i)
-            i += step
+            i += 1
         }
 
         while (i >= start && distance == -1) {
             distance = getDistance(i)
-            i -= step
+            i -= 1
         }
 
         return distance
     }
 
+    private fun getRange(start: Int, stop: Int): IntArray {
+        val result = IntArray(stop - start + 1)
+        var i = 0
+        while (i < stop - start + 1) {
+            result[i] = getDistance(start + i)
+            i++
+        }
+
+        return result
+    }
+
+
     fun calibrate() {
         getDistance(180)
     }

car_fmw/src/include/utils.kt

@@ -0,0 +1,4 @@
+
+fun positive(i: Int): Boolean {
+    return i > 0
+}

server/src/main/java/RoomScanner/CarController.kt

@@ -16,15 +16,14 @@ class CarController(var car: Car) {
         RIGHT(3);
     }
 
-    val WALL_DISTANCE = 50
-
     var position = Pair(0.0, 0.0)
         private set
 
+    private val WALL_DISTANCE = 50
     private var angle = 0.0
 
     fun moveTo(to: Pair<Double, Double>) {
-        var driveAngle = (Math.toDegrees(Math.atan2(to.first, to.second)) + 360) % 360
+        val driveAngle = (Math.toDegrees(Math.atan2(to.first, to.second)) + 360) % 360
         rotateOn(angleDistance(angle, driveAngle))
         drive(Direction.FORWARD, Math.max(0.0, distance(position, to)).toInt() - WALL_DISTANCE)
 
@@ -39,12 +38,6 @@ class CarController(var car: Car) {
         angle = target
     }
 
-    private fun drive(direction: Direction, distance: Int) {
-        val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf(distance), intArrayOf(direction.id)).build()
-        val data = serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
-        CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get()
-    }
-
     fun scan(angles: IntArray): List<Pair<Double, Double>> {
         val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 10, SonarRequest.Smoothing.MEDIAN).build()
         val data = serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
@@ -55,7 +48,13 @@ class CarController(var car: Car) {
         return distances.mapIndexed { i: Int, distance: Int -> convertToPoint(angles[i].toDouble(), distance.toDouble()) }
     }
 
-    fun convertToPoint(angle: Double, distance: Double): Pair<Double, Double> {
+    private fun drive(direction: Direction, distance: Int) {
+        val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf(distance), intArrayOf(direction.id)).build()
+        val data = serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
+        CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get()
+    }
+
+    private fun convertToPoint(angle: Double, distance: Double): Pair<Double, Double> {
         if (distance <= 0) {
             return Pair(0.0, 0.0)
         }
@@ -64,7 +63,7 @@ class CarController(var car: Car) {
         return Pair(Math.cos(realAngle) * distance + position.first, Math.sin(realAngle) * distance + position.second)
     }
 
-    inline fun serialize(size: Int, writeTo: (CodedOutputStream) -> Unit): ByteArray {
+    private inline fun serialize(size: Int, writeTo: (CodedOutputStream) -> Unit): ByteArray {
         val bytes = ByteArray(size)
         writeTo(CodedOutputStream(bytes))
 

server/src/main/java/RoomScanner/RoomScanner.kt

@@ -2,6 +2,7 @@ package RoomScanner
 
 class RoomScanner(val controller: CarController) : Thread() {
     private val points = mutableListOf<Pair<Double, Double>>()
+    private val MAX_VALID_DISTANCE = 100
 
     override fun run() {
         println("Car connected ${controller.car.host}")
@@ -20,7 +21,7 @@ class RoomScanner(val controller: CarController) : Thread() {
         controller.rotateOn(180.0)
         iterationPoints.addAll(controller.scan(horizon))
 
-        points.addAll(iterationPoints)
+        points.addAll(iterationPoints.filter { it.first > 0 && it.second > 0 && distance(controller.position, it) <= MAX_VALID_DISTANCE })
 
         val target = iterationPoints.filter { it.first > 0 && it.second > 0 }.maxBy { distance(controller.position, it) }
         target ?: return