car_srv: fix rotation angle and drive, working room scanner on emulator without noise

car_srv/kotlinSrv/src/CarState.kt

@@ -14,6 +14,7 @@ class CarState private constructor() {
     fun moving(distance: Int) {
         x += (Math.cos(degreesToRadian(angle)) * distance).toInt()
         y += (Math.sin(degreesToRadian(angle)) * distance).toInt()
+        println("new position: {$x, $y}")
     }
 
     private fun degreesToRadian(angleInDegrees: Int): Double {
@@ -23,6 +24,7 @@ class CarState private constructor() {
     //angle positive - rotation left
     fun rotate(angle: Int) {
         this.angle += angle
+        println("new angle: {$angle, ${this.angle}}")
     }
 
     companion object {

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

@@ -21,25 +21,25 @@ class CarController(var car: Car) {
     private val CHARGE_CORRECTION = 1.0 //0.85
     private var angle = 0.0
 
-    fun moveTo(to: Pair<Double, Double>) {
+    fun moveTo(to: Pair<Double, Double>, distance: Double) {
         val driveAngle = (Math.toDegrees(Math.atan2(to.first, to.second)) + 360) % 360
-        rotateOn(angleDistance(angle, driveAngle))
-        drive(Direction.FORWARD, 1)
+        rotateOn(driveAngle)
+        drive(Direction.FORWARD, distance.toInt())
 
-        position = Pair(position.first +  Math.cos(Math.toRadians(angle)),
-                position.second + Math.sin(Math.toRadians(angle)))
+        position = Pair(position.first +  distance * Math.cos(Math.toRadians(driveAngle)),
+                position.second + distance * Math.sin(Math.toRadians(driveAngle)))
     }
 
     fun rotateOn(target: Double) {
         val rotateAngle = angleDistance(angle, target)
         val direction = if (rotateAngle > 0) Direction.LEFT else Direction.RIGHT
-        drive(direction, rotateAngle.toInt())
+        drive(direction, Math.abs(rotateAngle.toInt()))
 
         angle = target
     }
 
     fun scan(angles: IntArray): List<Double> {
-        val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 5, SonarRequest.Smoothing.NONE).build()
+        val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 1, SonarRequest.Smoothing.NONE).build()
         val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
         val response = CarClient.sendRequest(car, CarClient.Request.SONAR, data).get().responseBodyAsBytes
 

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

@@ -12,35 +12,31 @@ class RoomScanner(val controller: CarController) : Thread() {
     }
 
     private fun step() {
-        val iterationPoints = scan()
-        points.addAll(iterationPoints.filter { it.first > 0 || it.second > 0 })
+        val iterationPoints = scan().filter { it.first > 0 || it.second > 0}
+        points.addAll(iterationPoints)
 
-        val target = iterationPoints.filter { it.first > 0 && it.second > 0 }.maxBy { distance(controller.position, it) }
+        val target = iterationPoints.maxBy { distance(controller.position, it) }
         target ?: return
 
-        controller.moveTo(target)
+        controller.moveTo(target, 50.0)
+        controller.rotateOn(0.0)
     }
 
     private fun scan(): MutableList<Pair<Double, Double>> {
         val horizon = IntArray(180 / 5, { it * 5 })
+        horizon.reverse()
 
-        val distance = fun(first: Double, second: Double): Double = when {
-            first == second -> 0.0
-            first == -1.0 -> second
-            second == -1.0 -> first
-            else -> Math.abs(first - second)
+        val dots = mutableListOf<Pair<Double, Double>>()
+        for (i in arrayOf(0.0, 90.0, 180.0, 270.0)) {
+            controller.rotateOn(i)
+            dots.addAll(controller.scan(horizon).mapIndexed { i: Int, d: Double -> controller.convertToPoint((i * 5 - 180).toDouble(), d) })
         }
 
-        val metrics = mutableListOf<List<Double>>()
-        for (i in intArrayOf(0, 180)) {
-            controller.rotateOn(i.toDouble())
-            metrics.add(controller.scan(horizon))
-        }
+        controller.rotateOn(0.0)
 
-        val points = merge(metrics.reversed(), distance).reversed()
-        return points.mapIndexed { i: Int, d: Double -> controller.convertToPoint((i * 5).toDouble(), d) }.toMutableList()
+        return dots
     }
 
     private fun plot(points: MutableList<Pair<Double, Double>>) =
-            "x <- c(${points.joinToString { it.first.toInt().toString() }}) \n y <- c(${points.joinToString { it.second.toInt().toString() }})"
+            "x <- c(${points.joinToString { it.first.toInt().toString() }}) \ny <- c(${points.joinToString { it.second.toInt().toString() }})"
 }

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

@@ -11,13 +11,10 @@ fun estimateAngle(from: Pair<Double, Double>, to: Pair<Double, Double>): Double
         Math.atan2(to.first - from.first, to.second - from.second)
 
 fun angleDistance(from: Double, to: Double): Double {
-    val min = Math.min(from, to)
-    val max = Math.max(from, to)
+    val distance = Math.min(Math.abs(from - to), Math.abs(360 - (from - to)))
+    val direction = if (distance == Math.abs(from - to)) Math.signum(to - from) else -Math.signum(to - from)
 
-    val up = max - min
-    val down = 360 - max + min
-
-    if (Math.abs(up) < Math.abs(down)) return up else return down
+    return distance * direction
 }
 
 fun <T> maxSuffix(first: List<T>, second: List<T>, distance: (T, T) -> Double): Int {