From 5c3c63a60a60628548f69da1046fa4dc9c5b61d5 Mon Sep 17 00:00:00 2001 From: e5l Date: Wed, 31 Aug 2016 18:09:47 +0300 Subject: [PATCH] car_srv: horizon merge, fix rotation, minor fixes --- car_hw/src/car_sonar.c | 2 +- .../main/java/RoomScanner/CarController.kt | 24 +++++----- .../src/main/java/RoomScanner/RoomScanner.kt | 44 ++++++++++++++----- server/src/main/java/RoomScanner/utils.kt | 36 ++++++++++++++- server/src/main/java/utils.kt | 0 5 files changed, 79 insertions(+), 27 deletions(-) delete mode 100644 server/src/main/java/utils.kt diff --git a/car_hw/src/car_sonar.c b/car_hw/src/car_sonar.c index 9bb854f3b78..d1df61cb002 100644 --- a/car_hw/src/car_sonar.c +++ b/car_hw/src/car_sonar.c @@ -49,7 +49,7 @@ static uint16_t sonar_wait_dist_resp(void) size_t i = 0; uint8_t sum = 0; - uint32_t distance = 0; + int16_t distance = 0; usart_rcv_data(USART3_ID, sonar_resp, 4); sum = sonar_resp[0] + sonar_resp[1] + sonar_resp[2]; diff --git a/server/src/main/java/RoomScanner/CarController.kt b/server/src/main/java/RoomScanner/CarController.kt index 5c7f269643c..228fb5c636e 100644 --- a/server/src/main/java/RoomScanner/CarController.kt +++ b/server/src/main/java/RoomScanner/CarController.kt @@ -19,6 +19,7 @@ class CarController(var car: Car) { private set private val WALL_DISTANCE = 50 + private val CHARGE_CORRECTION = 1.0 //0.85 private var angle = 0.0 fun moveTo(to: Pair) { @@ -31,29 +32,21 @@ class CarController(var car: Car) { fun rotateOn(target: Double) { val rotateAngle = angleDistance(angle, target) - val direction = if (rotateAngle > 0) Direction.RIGHT else Direction.LEFT + val direction = if (rotateAngle > 0) Direction.LEFT else Direction.RIGHT drive(direction, rotateAngle.toInt()) angle = target } - fun scan(angles: IntArray): List> { - val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN).build() + fun scan(angles: IntArray): List { + val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 5, 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 - val distances = SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances - - return distances.mapIndexed { i: Int, distance: Int -> convertToPoint(angles[i].toDouble(), distance.toDouble()) } + return SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances.map { it.toDouble() } } - private fun drive(direction: Direction, distance: Int) { - val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf(distance), intArrayOf(direction.id)).build() - val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) }) - CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get() - } - - private fun convertToPoint(angle: Double, distance: Double): Pair { + fun convertToPoint(angle: Double, distance: Double): Pair { if (distance <= 0) { return Pair(0.0, 0.0) } @@ -62,5 +55,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) { + val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf((distance * CHARGE_CORRECTION).toInt()), intArrayOf(direction.id)).build() + val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) }) + CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get() + } } diff --git a/server/src/main/java/RoomScanner/RoomScanner.kt b/server/src/main/java/RoomScanner/RoomScanner.kt index 4dbdf5f4144..337c64d00c8 100644 --- a/server/src/main/java/RoomScanner/RoomScanner.kt +++ b/server/src/main/java/RoomScanner/RoomScanner.kt @@ -6,22 +6,20 @@ class RoomScanner(val controller: CarController) : Thread() { override fun run() { println("Car connected ${controller.car.host}") + var i = 1 while (true) { - scan() - println(plot(points)) + step() + + if (i % 10 == 0) { + println(plot(points)) + } + i++ } } - private fun scan() { - val horizon = IntArray(180 / 5, { it * 5 }) - val iterationPoints = mutableListOf>() - - controller.rotateOn(0.0) - iterationPoints.addAll(controller.scan(horizon)) - controller.rotateOn(180.0) - iterationPoints.addAll(controller.scan(horizon)) - - points.addAll(iterationPoints.filter { it.first > 0 && it.second > 0 && distance(controller.position, it) <= MAX_VALID_DISTANCE }) + private fun step() { + val iterationPoints = scan() + points.addAll(iterationPoints.filter { it.first > 0 && it.second > 0 }) val target = iterationPoints.filter { it.first > 0 && it.second > 0 }.maxBy { distance(controller.position, it) } target ?: return @@ -29,6 +27,28 @@ class RoomScanner(val controller: CarController) : Thread() { controller.moveTo(target) } + private fun scan(): MutableList> { + val horizon = IntArray(180 / 5, { it * 5 }) + + 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 metrics = mutableListOf>() + for (i in intArrayOf(0, 90, 180, 270)) { + controller.rotateOn(i.toDouble()) + metrics.add(controller.scan(horizon)) + } + + val points = merge(metrics.reversed(), distance).reversed() + + controller.rotateOn(0.0) + return points.mapIndexed { i: Int, d: Double -> controller.convertToPoint((i * 5).toDouble(), d) }.toMutableList() + } + private fun plot(points: MutableList>) = "x <- c(${points.joinToString { it.first.toInt().toString() }}) \n y <- c(${points.joinToString { it.second.toInt().toString() }})" } diff --git a/server/src/main/java/RoomScanner/utils.kt b/server/src/main/java/RoomScanner/utils.kt index fea1145df2e..058c066863c 100644 --- a/server/src/main/java/RoomScanner/utils.kt +++ b/server/src/main/java/RoomScanner/utils.kt @@ -18,4 +18,38 @@ fun angleDistance(from: Double, to: Double): Double { val down = 360 - max + min if (Math.abs(up) < Math.abs(down)) return up else return down -} \ No newline at end of file +} + +fun maxSuffix(first: List, second: List, distance: (T, T) -> Double): Int { + val distances = first.mapIndexed { i: Int, t: T -> + var score = 0.0 + var size = Math.min(second.size - 1, first.size - i - 1) + for (j in 0..size) { + score += distance(first[i + j], second[j]) + } + + score / (size + 1) + }.toList() + + return distances.indexOf(distances.min()) +} + +fun merge(lists: List>, distance: (T, T) -> Double): List { + val indexes = mutableListOf() + for (i in 0..(lists.size - 2)) { + indexes.add(maxSuffix(lists[i], lists[i + 1], distance)) + } + + val result = mutableListOf() + for (i in 0..(indexes.size - 1)) { + for (j in 0..indexes[i]) { + result.add(lists[i][j]) + } + } + + for (i in (indexes.last() + 1)..(lists.last().size - 1)) { + result.add(lists.last()[i]) + } + + return result +} diff --git a/server/src/main/java/utils.kt b/server/src/main/java/utils.kt deleted file mode 100644 index e69de29bb2d..00000000000