From dc16f3e4806c9c2e3b71de1fbdf73f086a17d0b9 Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Mon, 29 Aug 2016 12:08:39 +0300 Subject: [PATCH] algorithm refactoring --- server/src/main/java/DebugClInterface.kt | 3 +- .../main/java/algorithm/AbstractAlgorithm.kt | 46 +++++----- .../java/algorithm/RoomBypassingAlgorithm.kt | 84 +++++++++---------- server/src/main/java/algorithm/RoomModel.kt | 19 ++--- server/src/main/java/algorithm/RoomTest.kt | 25 +++--- .../main/java/algorithm/geometry/AngleData.kt | 19 +++++ .../src/main/java/algorithm/geometry/Point.kt | 4 + .../src/main/java/algorithm/geometry/Util.kt | 14 ++++ 8 files changed, 116 insertions(+), 98 deletions(-) create mode 100644 server/src/main/java/algorithm/geometry/AngleData.kt create mode 100644 server/src/main/java/algorithm/geometry/Point.kt create mode 100644 server/src/main/java/algorithm/geometry/Util.kt diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index 566434bd832..90b5cdf4cd8 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -1,4 +1,3 @@ - import Exceptions.InactiveCarException import algorithm.AbstractAlgorithm import algorithm.RoomBypassingAlgorithm @@ -189,7 +188,7 @@ object DebugClInterface { when (command) { "reset" -> return null "done" -> { - val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray()) + val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, {1}), 0, SonarRequest.Smoothing.NONE) return sonarBuilder.build() } else -> { diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index b8f2de5dfcd..1a185eee47b 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -4,6 +4,7 @@ import CodedOutputStream import Exceptions.InactiveCarException import RouteMetricRequest import SonarRequest +import algorithm.geometry.AngleData import io.netty.buffer.Unpooled import io.netty.handler.codec.http.* import objects.Car @@ -24,7 +25,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() + private var prevSonarDistances = mapOf() private val defaultAngles = listOf(0, 60, 90, 120, 180).toIntArray() protected var requiredAngles = defaultAngles @@ -34,43 +35,36 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() + val anglesDistances = mutableMapOf() for (i in 0..angles.size - 1) { if (Math.abs(distances[i]) < 0.01) { continue } - anglesDistances.put(angles[i], distances[i]) + anglesDistances.put(angles[i], AngleData(angles[i], distances[i])) } this.requiredAngles = defaultAngles @@ -133,7 +127,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger { + protected fun getPrevSonarDistances(): Map { return prevSonarDistances } @@ -141,8 +135,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger): CarState? - protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest + protected abstract fun getCarState(anglesDistances: Map): CarState? + protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest protected abstract fun afterGetCommand(route: RouteMetricRequest) diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 32d21b63eff..0108641dd47 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -1,15 +1,14 @@ package algorithm -import algorithm.geometry.Line -import algorithm.geometry.Vector import objects.Car import RouteMetricRequest +import algorithm.geometry.* import java.util.concurrent.Exchanger class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { - public var wallAngleWithOX = 0.0//in radian - public var wallLength = 0.0//sm + var wallAngleWithOX = 0.0//in radian + var wallLength = 0.0//sm var errorCount = 0 var carX = 0 @@ -18,17 +17,18 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs private var prevPoint = Pair(0.0, 0.0) - override fun getCarState(anglesDistances: Map): CarState? { - val dist0 = anglesDistances[0] - val dist90 = anglesDistances[90] - if (dist90 == null || dist90 > 85) { + override fun getCarState(anglesDistances: Map): CarState? { + val angleData0 = anglesDistances[0] + val angleData90 = anglesDistances[90] + if (angleData90 == null || angleData90.distance > 85) { requiredAngles = getIntArray(0, 60, 90, 120, 180) return CarState.OUTER } + val dist90 = angleData90.distance if (dist90 < 20) { return CarState.WALL } - if (dist0 == null || dist0 > 70) { + if (angleData0 == null || angleData0.distance > 70) { return CarState.WALL } return CarState.INNER @@ -59,14 +59,14 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } } - override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest { - val dist0 = anglesDistances[0] - val dist60 = anglesDistances[60] - val dist90 = anglesDistances[90] - val dist120 = anglesDistances[120] - val dist180 = anglesDistances[180] + override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest { + val angleData0 = anglesDistances[0] + val angleData60 = anglesDistances[60] + val angleData90 = anglesDistances[90] + val angleData120 = anglesDistances[120] + val angleData180 = anglesDistances[180] val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) - if (dist120 == null || dist90 == null || dist60 == null) { + if (angleData120 == null || angleData90 == null || angleData60 == null) { println("null distance!") if (errorCount >= 3) { errorCount = 0 @@ -79,26 +79,26 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } if (getPrevState() == null) { //its first run, save right wall - RoomModel.lines.add(Line(0.0, 1.0, dist90)) - prevPoint = Pair(0.0, -dist90) - if (dist0 != null) { - wallLength = dist0 + RoomModel.lines.add(Line(0.0, 1.0, angleData90.distance.toDouble())) + prevPoint = Pair(0.0, -angleData90.distance.toDouble()) + if (angleData0 != null) { + wallLength = angleData0.distance.toDouble() } } when (state) { CarState.WALL -> { - if (wallLength.toInt() == 0 && (dist0 != null && dist180 != null)) { - wallLength = dist0 + dist180 + if (wallLength.toInt() == 0 && (angleData0 != null && angleData180 != null)) { + wallLength = angleData0.distance.toDouble() + angleData180.distance.toDouble() } - if (dist90 > 40 || dist90 < 20) { - val rotationDirection = if (dist90 > 40) RIGHT else LEFT + if (angleData90.distance > 40 || angleData90.distance < 20) { + val rotationDirection = if (angleData90.distance > 40) RIGHT else LEFT resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) resultBuilder.setDistances(getIntArray(10, 35)) return resultBuilder.build() } - if (Math.abs(dist120 - dist60) > 10) { - val rotationDirection = if (dist120 > dist60) LEFT else RIGHT + if (Math.abs(angleData120.distance - angleData60.distance) > 10) { + val rotationDirection = if (angleData120.distance > angleData60.distance) LEFT else RIGHT resultBuilder.setDirections(getIntArray(rotationDirection)) resultBuilder.setDistances(getIntArray(15)) return resultBuilder.build() @@ -111,18 +111,18 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs CarState.INNER -> { //в угле первое действие - встать максимально паралельно стене. - if (Math.abs(dist120 - dist60) > 10) { - val rotationDirection = if (dist120 > dist60) LEFT else RIGHT + if (Math.abs(angleData120.distance - angleData60.distance) > 10) { + val rotationDirection = if (angleData120.distance > angleData60.distance) LEFT else RIGHT resultBuilder.setDirections(getIntArray(rotationDirection)) resultBuilder.setDistances(getIntArray(12)) return resultBuilder.build() } //если стоим паралельно - можно мерить угол - val pointsNextLine = arrayListOf>() - for (angleDist in anglesDistances) { - if (angleDist.key <= 15) { - pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair())) + val pointsNextLine = arrayListOf() + for (angleData in anglesDistances.values) { + if (angleData.angle <= 15) { + pointsNextLine.add(angleData.toPoint(Util.radianToDegrees(wallAngleWithOX))) } } if (pointsNextLine.size < 3) { @@ -178,30 +178,22 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs return angle * 180 / Math.PI } - private fun sonarAngleDistToPoint(angleDist: Pair): Pair { - val angle = wallAngleWithOX - degreesToRadian(angleDist.first) - val dist = angleDist.second - - val result = Pair(Math.cos(angle) * dist, Math.sin(angle) * dist) - return result - } - - private fun approximatePointsByLine(points: Array>): Line { + private fun approximatePointsByLine(points: Array): Line { // val p1 = points.first() // val p2 = points[1] // return Line(p2.second - p1.second, p1.first - p2.first, -p1.first * p2.second + p1.second * p2.first) - val sumX = points.sumByDouble { it.first } - val sumXQuad = points.sumByDouble { it.first * it.first } - val sumY = points.sumByDouble { it.second } - val sumXY = points.sumByDouble { it.second * it.first } + val sumX = points.sumByDouble { it.x } + val sumXQuad = points.sumByDouble { it.x * it.x } + val sumY = points.sumByDouble { it.y } + val sumXY = points.sumByDouble { it.y * it.x } val pointsCount = points.size val den = pointsCount * sumXQuad - sumX * sumX if (Math.abs(den) < 0.001) { - return Line(1.0, 0.0, -points.first().first) + return Line(1.0, 0.0, -points.first().x) } val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX) val b = (sumY - sumX * k) / pointsCount diff --git a/server/src/main/java/algorithm/RoomModel.kt b/server/src/main/java/algorithm/RoomModel.kt index 936dc8b4f8f..0a8ffeccfa2 100644 --- a/server/src/main/java/algorithm/RoomModel.kt +++ b/server/src/main/java/algorithm/RoomModel.kt @@ -35,23 +35,14 @@ object RoomModel { val end_y = IntArray(algorithm.points.size) - for (i in 0..algorithm.points.size - 1) { + for (i in 0..algorithm.points.size - 2) { val curPoint = algorithm.points[i] - if (algorithm.points.size - 1 == i) { -// val nextPoint = algorithm.points[0] -// begin_x[i] = curPoint.first.toInt() -// begin_y[i] = curPoint.second.toInt() -// -// end_x[i] = nextPoint.first.toInt() -// end_y[i] = nextPoint.second.toInt() - continue - } val nextPoint = algorithm.points[i + 1] - begin_x[i] = curPoint.first.toInt() - begin_y[i] = curPoint.second.toInt() + begin_x[i] = (curPoint.x + 0.5).toInt() + begin_y[i] = (curPoint.y + 0.5).toInt() - end_x[i] = nextPoint.first.toInt() - end_y[i] = nextPoint.second.toInt() + end_x[i] = (nextPoint.x + 0.5).toInt() + end_y[i] = (nextPoint.y + 0.5).toInt() } return Waypoints.BuilderWaypoints(begin_x, begin_y, end_x, end_y, false).build() } diff --git a/server/src/main/java/algorithm/RoomTest.kt b/server/src/main/java/algorithm/RoomTest.kt index 88970b85ea6..5d1d3a93ad2 100644 --- a/server/src/main/java/algorithm/RoomTest.kt +++ b/server/src/main/java/algorithm/RoomTest.kt @@ -3,20 +3,23 @@ package algorithm import objects.Car import java.util.concurrent.Exchanger import RouteMetricRequest +import algorithm.geometry.AngleData +import algorithm.geometry.Point class RoomTest(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { - val points = arrayListOf>() + val points = arrayListOf() + private val DISTANCE_TO_WALL_THRESHOLD = 60 var carX = 0 var carY = 0 var carAngle = 0 - override fun getCarState(anglesDistances: Map): CarState { + override fun getCarState(anglesDistances: Map): CarState { return CarState.WALL } - override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest { + override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest { val dist0 = anglesDistances[0] val dist60 = anglesDistances[60] val dist90 = anglesDistances[90] @@ -28,27 +31,29 @@ class RoomTest(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm return resultBuilder.build() } val sonarAngle = carAngle - 90 - points.add(Pair(carX + dist90 * Math.cos(degreesToRadian(sonarAngle)), - carY + dist90 * Math.sin(degreesToRadian(sonarAngle)))) + points.add(Point( + x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)), + y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle)) + )) if (dist120 == null || dist60 == null) { resultBuilder.setDirections(getIntArray(FORWARD)) resultBuilder.setDistances(getIntArray(10)) return resultBuilder.build() } - if (dist0 != null && dist0 < 60) { + if (dist0 != null && dist0.distance < DISTANCE_TO_WALL_THRESHOLD) { resultBuilder.setDirections(getIntArray(LEFT, FORWARD)) resultBuilder.setDistances(getIntArray(20, 10)) return resultBuilder.build() } - val abs = Math.abs(dist120 - dist60) + val abs = Math.abs(dist120.distance - dist60.distance) if (abs > 10) { - val rotationDirection = if (dist120 > dist60) LEFT else RIGHT + val rotationDirection = if (dist120.distance > dist60.distance) LEFT else RIGHT resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate return resultBuilder.build() } - if (dist90 > 50 || dist90 < 25) { - val rotationDirection = if (dist90 > 50) RIGHT else LEFT + if (dist90.distance > 50 || dist90.distance < 25) { + val rotationDirection = if (dist90.distance > 50) RIGHT else LEFT resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) resultBuilder.setDistances(getIntArray(10, 10)) return resultBuilder.build() diff --git a/server/src/main/java/algorithm/geometry/AngleData.kt b/server/src/main/java/algorithm/geometry/AngleData.kt new file mode 100644 index 00000000000..b2e69aede2a --- /dev/null +++ b/server/src/main/java/algorithm/geometry/AngleData.kt @@ -0,0 +1,19 @@ +package algorithm.geometry + + +class AngleData(val angle: Int, val distance: Int) { + + + fun toPoint(carAngleOX: Int): Point { + + //convert to global coordinate system + val angle = carAngleOX - angle + val radianAngle = Util.degreesToRadian(angle) + + return Point( + x = Math.cos(radianAngle) * distance, + y = Math.sin(radianAngle) * distance + ) + } + +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Point.kt b/server/src/main/java/algorithm/geometry/Point.kt new file mode 100644 index 00000000000..40b317e5bb1 --- /dev/null +++ b/server/src/main/java/algorithm/geometry/Point.kt @@ -0,0 +1,4 @@ +package algorithm.geometry + +class Point(val x: Double, val y: Double) { +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Util.kt b/server/src/main/java/algorithm/geometry/Util.kt new file mode 100644 index 00000000000..b6d0aaa0a24 --- /dev/null +++ b/server/src/main/java/algorithm/geometry/Util.kt @@ -0,0 +1,14 @@ +package algorithm.geometry + +object Util { + + + fun degreesToRadian(angleDegrees: Int): Double { + return Math.PI * angleDegrees / 180.0 + } + + fun radianToDegrees(angleRad: Double): Int { + return (180.0 * angleRad / Math.PI).toInt() + } + +} \ No newline at end of file