car_srv: rotate correction with sonar

car_srv/kotlinSrv/src/CarState.kt

@@ -14,7 +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}")
+        println("new position: {$x, $y}, distance: {$distance}")
     }
 
     private fun degreesToRadian(angleInDegrees: Int): Double {
@@ -23,7 +23,7 @@ class CarState private constructor() {
 
     //angle positive - rotation left
     fun rotate(angle: Int) {
-        this.angle += angle
+        this.angle = (this.angle + angle) % 360
         println("new angle: {$angle, ${this.angle}}")
     }
 

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

@@ -18,15 +18,20 @@ class CarController(var car: Car) {
     var position = Pair(0.0, 0.0)
         private set
 
-    private val CHARGE_CORRECTION = 1.0 //0.85
+    private val CHARGE_CORRECTION = 0.97
+    private val MAX_ANGLE = 360.0
+    private val SCAN_STEP = 5.0
+    private val MIN_ROTATION = 10
+    private val MAX_VALID_DISTANCE = 100.0
+
     private var angle = 0.0
 
     fun moveTo(to: Pair<Double, Double>, distance: Double) {
-        val driveAngle = (Math.toDegrees(Math.atan2(to.first, to.second)) + 360) % 360
+        val driveAngle = (Math.toDegrees(Math.atan2(to.second, to.first)) + MAX_ANGLE) % MAX_ANGLE
         rotateOn(driveAngle)
         drive(Direction.FORWARD, distance.toInt())
 
-        position = Pair(position.first +  distance * Math.cos(Math.toRadians(driveAngle)),
+        position = Pair(position.first + distance * Math.cos(Math.toRadians(driveAngle)),
                 position.second + distance * Math.sin(Math.toRadians(driveAngle)))
     }
 
@@ -38,12 +43,38 @@ class CarController(var car: Car) {
         angle = target
     }
 
+    fun rotateLeftWithCorrection(angle: Double) {
+        val horizon = horizon()
+        val before = scan(horizon)
+        drive(Direction.LEFT, angle.toInt())
+        var after = scan(horizon)
+
+        val distance = fun(first: Double, second: Double): Double = Math.max(when {
+            first == second -> 0.0
+            first == -1.0 -> second
+            second == -1.0 -> first
+            else -> Math.max(Math.abs(first - second).toDouble(), 100.0)
+        }, MAX_VALID_DISTANCE)
+
+        var realAngle = (maxSuffix(before, after, distance) + 1) * SCAN_STEP
+        while (realAngle != angle) {
+            val direction = if (realAngle > angle) Direction.RIGHT else Direction.LEFT
+            drive(direction, Math.min(MIN_ROTATION, Math.abs(realAngle - angle).toInt()))
+
+            after = scan(horizon)
+            realAngle = maxSuffix(before, after, distance) * SCAN_STEP
+        }
+
+        this.angle = realAngle
+    }
+
     fun scan(angles: IntArray): List<Double> {
         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
 
-        return SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances.map { it.toDouble() }
+        val result = SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances.map { it.toDouble() }
+        return result
     }
 
     fun convertToPoint(angle: Double, distance: Double): Pair<Double, Double> {
@@ -55,10 +86,10 @@ class CarController(var car: Car) {
         return Pair(Math.cos(realAngle) * distance + position.first, Math.sin(realAngle) * distance + position.second)
     }
 
-    private fun drive(direction: Direction, distance: Int) {
+    fun drive(direction: Direction, distance: Int) {
         val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf((distance * CHARGE_CORRECTION).toInt()), intArrayOf(direction.id)).build()
         val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
-        val response = CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get()
+        CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get()
     }
 
 }

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

@@ -2,38 +2,39 @@ package RoomScanner
 
 class RoomScanner(val controller: CarController) : Thread() {
     private val points = mutableListOf<Pair<Double, Double>>()
+    private val VALID_MEASURE = 150
+    private val GHOST_LEVEL = 50
 
     override fun run() {
         println("Car connected ${controller.car.host}")
-        while (true) {
+        for (i in 1..100) {
             step()
             println(plot(points))
         }
+
     }
 
     private fun step() {
-        val iterationPoints = scan().filter { it.first > 0 || it.second > 0}
-        points.addAll(iterationPoints)
+        val iterationPoints = scan().filter { it.first > 0 || it.second > 0 }
+        points.addAll(iterationPoints.filter { distance(controller.position, it) <= VALID_MEASURE })
 
-        val target = iterationPoints.maxBy { distance(controller.position, it) }
-        target ?: return
-
-        controller.moveTo(target, 50.0)
+        val target = iterationPoints.sortedBy { distance(controller.position, it) }.takeLast(GHOST_LEVEL)[(Math.random() * GHOST_LEVEL).toInt()]
+        controller.moveTo(target, Math.min(distance(controller.position, target), 100.0))
         controller.rotateOn(0.0)
     }
 
     private fun scan(): MutableList<Pair<Double, Double>> {
-        val horizon = IntArray(180 / 5, { it * 5 })
-        horizon.reverse()
+        val horizon = horizon()
 
         val dots = mutableListOf<Pair<Double, Double>>()
         for (i in arrayOf(0.0, 90.0, 180.0, 270.0)) {
-            controller.rotateOn(i)
+            controller.rotateLeftWithCorrection(90.0)
             dots.addAll(controller.scan(horizon).mapIndexed { i: Int, d: Double -> controller.convertToPoint((i * 5 - 180).toDouble(), d) })
+            controller.drive(CarController.Direction.FORWARD, 10)
+            controller.drive(CarController.Direction.BACKWARD, 10)
         }
 
-        controller.rotateOn(0.0)
-
+        controller.rotateLeftWithCorrection(90.0)
         return dots
     }
 

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

@@ -1,5 +1,12 @@
 package RoomScanner
 
+fun horizon(): IntArray {
+    val horizon = IntArray(180 / 5, { it * 5 })
+    horizon.reverse()
+
+    return horizon
+}
+
 fun distance(first: Pair<Double, Double>, second: Pair<Double, Double>): Double {
     val xDistance = first.first - second.first
     val yDistance = first.second - second.second
@@ -7,9 +14,6 @@ fun distance(first: Pair<Double, Double>, second: Pair<Double, Double>): Double
     return Math.sqrt(xDistance * xDistance + yDistance * yDistance)
 }
 
-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 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)