diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index c045f61e87d..3feab032994 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -64,7 +64,7 @@ object DebugClInterface { "refloc" -> executeRefreshLocationCommand() "sonar" -> executeSonarCommand(readString) "dbinfo" -> executeDebugInfoCommand(readString) - "lines" -> println(algorithm.RoomModel.lines) + "lines" -> algorithm.RoomModel.walls.forEach { println(it.line) } "pos" -> { val tmp = algorithmImpl if (tmp is RoomBypassingAlgorithm) { @@ -207,7 +207,7 @@ object DebugClInterface { when (command) { "reset" -> return null "done" -> { - val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 1 }), 0, SonarRequest.Smoothing.NONE) + val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN) return sonarBuilder.build() } else -> { diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 67d1f77c532..b8baa19bfec 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -19,8 +19,8 @@ import java.util.concurrent.TimeoutException abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger) { open val ATTEMPTS = 1 - open val THRESHOLD = 0 open val SMOOTHING = SonarRequest.Smoothing.NONE + open val WINDOW_SIZE = 0 protected val FORWARD = 0 protected val BACKWARD = 1 @@ -30,7 +30,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() - private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(90), Angle(120), Angle(180)) + private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(70), Angle(80), Angle(90), Angle(100), Angle(110), Angle(120), Angle(180)) protected var requiredAngles = defaultAngles protected enum class CarState { @@ -45,8 +45,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() for (i in 0..angles.size - 1) { - if (Math.abs(distances[i]) < 0.01) { - continue - } anglesDistances.put(angles[i], AngleData(angles[i], distances[i])) } diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 939fcd20fad..7c13d444309 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -1,6 +1,7 @@ package algorithm import RouteMetricRequest +import SonarRequest import algorithm.geometry.Angle import algorithm.geometry.AngleData import algorithm.geometry.Point @@ -11,14 +12,19 @@ import java.util.concurrent.Exchanger class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { // TODO: set to appropriate values - override val THRESHOLD = 0 - override val ATTEMPTS: Int = 3 + override val ATTEMPTS: Int = 5 override val SMOOTHING = SonarRequest.Smoothing.MEDIAN + override val WINDOW_SIZE = 3 - private val MAX_DISTANCE_TO_WALL_AHEAD = 60 - private val ISOSCALENESS_THRESHOLD = 10 - private val DISTANCE_TO_WALL_UPPER_BOUND = 50 - private val DISTANCE_TO_WALL_LOWER_BOUND = 25 + private val MAX_DISTANCE_TO_WALL_AHEAD = 30 // reached the corner and should turn left + private val OUTER_CORNER_DISTANCE_THRESHOLD = 60 // reached outer corner + private val ISOSCALENESS_MIN_DIFF = 10 // have to correct alignment to the wall + private val ISOSCALENESS_MAX_DIFF = 50 // almost reached outer corner but not yet, and 120 meas. founds far wall + private val DISTANCE_TO_WALL_UPPER_BOUND = 50 // have to move closer to the parallel wall + private val DISTANCE_TO_WALL_LOWER_BOUND = 25 // have to move farther to the parallel wall + private val SPURIOUS_REFLECTION_DIFF = 70 // we're approaching corner and get spurious reflection from two walls on 60 meas. ! Should be before outer corner case ! + + private var calibrateAfterRotate = false var carX = 0 var carY = 0 @@ -37,16 +43,21 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs private fun noOrthogonalMeasurementFound(anglesDistances: Map, state: CarState): RouteMetricRequest { val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) - resultBuilder.setDirections(getIntArray(RIGHT)) - resultBuilder.setDistances(getIntArray(10)) + resultBuilder.setDirections(getIntArray(FORWARD, RIGHT, FORWARD)) + val wallAngle = calculateAngle(anglesDistances, state) + resultBuilder.setDistances(getIntArray(15, wallAngle.degs(), 60)) + addWall(-wallAngle) + calibrateAfterRotate = true return resultBuilder.build() } private fun wallAheadFound(anglesDistances: Map, state: CarState): RouteMetricRequest { val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) - resultBuilder.setDirections(getIntArray(LEFT)) - resultBuilder.setDistances(getIntArray(calculateAngle(anglesDistances, state).degs())) - addWall() + resultBuilder.setDirections(getIntArray(LEFT, FORWARD)) + val wallAngle = calculateAngle(anglesDistances, state) + resultBuilder.setDistances(getIntArray(wallAngle.degs(), 15)) + addWall(wallAngle) + calibrateAfterRotate = true return resultBuilder.build() } @@ -71,31 +82,53 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs return resultBuilder.build() } - private fun addWall() { + private fun moveForward(): RouteMetricRequest { + val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + resultBuilder.setDirections(getIntArray(FORWARD)) + resultBuilder.setDistances(getIntArray(20)) + return resultBuilder.build() + } + + private fun addWall(angleWithPrevWall: Angle) { RoomModel.updateWalls() - if (RoomModel.walls.last() == RoomModel.walls.first() && RoomModel.walls.size > 1) { - println("YEAH!") + val firstWall = RoomModel.walls.first() + val lastWall = RoomModel.walls.last() + if (firstWall == lastWall && RoomModel.walls.size > 1) { RoomModel.finished = true - RoomModel.walls.removeAt(RoomModel.walls.size - 1) + lastWall.pushBackPoint(firstWall.rawPoints.last()) + lastWall.markAsFinished() + RoomModel.walls.removeAt(0) + } else { + RoomModel.walls.add(Wall(lastWall.wallAngleOX + angleWithPrevWall)) } - RoomModel.walls.add(Wall()) } override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest { - val dist0 = anglesDistances[Angle(0)] - val dist60 = anglesDistances[Angle(60)] - val dist90 = anglesDistances[Angle(90)] - val dist120 = anglesDistances[Angle(120)] + + val dist0 = anglesDistances[Angle(0)]!! + val dist60 = anglesDistances[Angle(60)]!! + val dist90 = anglesDistances[Angle(90)]!! + val dist120 = anglesDistances[Angle(120)]!! val sonarAngle = carAngle - 90 - // Check most basic measurements: 60/90/120 - if (dist90 == null) { - return noOrthogonalMeasurementFound(anglesDistances, state) + if (calibrateAfterRotate) { + if (Math.abs(dist60.distance - dist120.distance) > 4) { + println("Flaw in align found, correcting") + val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT + resultBuilder.setDirections(getIntArray(rotationDirection)) + resultBuilder.setDistances(getIntArray(5)) + return resultBuilder.build() + } else { + calibrateAfterRotate = false + carAngle = RoomModel.walls.last().wallAngleOX.degs() + } } - - if (dist120 == null || dist60 == null) { - return noParallelWallFound(anglesDistances, state) + // Check most basic measurements: 60/90/120 + if (dist90.distance == -1 || dist90.distance > OUTER_CORNER_DISTANCE_THRESHOLD) { + println("No orthogonal measurement found") + return noOrthogonalMeasurementFound(anglesDistances, state) } // Add point to room map @@ -103,31 +136,65 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)), y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle)) ) - RoomModel.walls.last().addPoint(point) + RoomModel.walls.last().pushBackPoint(point) // Check if corner reached - if (dist0 != null && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) { + if (dist0.distance != -1 && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) { + println("Wall ahead found") return wallAheadFound(anglesDistances, state) } - // Checks for paralleleness + // Big fail, try to do something safe + if (dist120.distance == -1 && dist60.distance == -1) { + println("No parallel wall found") + return noParallelWallFound(anglesDistances, state) + } - // Check if wall is not parallel - val diff = Math.abs(dist120.distance - dist60.distance) - if (diff > ISOSCALENESS_THRESHOLD) { + // Big fail too + if (dist120.distance == -1) { + println("No back measurement found, wtf") + return moveForward() + } + + // Approaching inner corner and getting spurious reflection from 2 walls on 60 - just move forward to get closer to corner + if (dist60.distance - dist0.distance > SPURIOUS_REFLECTION_DIFF) { + println("Spurious reflection detected, moving forward") + return moveForward() + } + + // Approaching outer corner (parallel wall is ending soon, but not yet) - just move forward to get to the end of the wall + if (dist60.distance == -1 || Math.abs(dist120.distance - dist60.distance) > ISOSCALENESS_MAX_DIFF) { + println("Aprroaching outer corner, moving forward") + return moveForward() + } + + RoomModel.walls.last().rawPoints.removeAt(RoomModel.walls.last().rawPoints.size - 1) + for (i in 60..120 step 10) { + val point = Point( + x = carX + anglesDistances[Angle(i)]!!.distance * Math.cos(degreesToRadian(carAngle - i)), + y = carY + anglesDistances[Angle(i)]!!.distance * Math.sin(degreesToRadian(carAngle - i)) + ) + RoomModel.walls.last().pushBackPoint(point) + } + + // Check if realignment need + if (Math.abs(dist60.distance - dist120.distance) > ISOSCALENESS_MIN_DIFF) { + println("Flaw in align found, correcting") return alignParallelToWall(anglesDistances, state) } +// car stay parallel with wall +// carAngle = RoomModel.walls.last().wallAngleOX.degs() + // Check if wall is too close or too far - if (dist90.distance > 50 || dist90.distance < 25) { + if (dist90.distance > DISTANCE_TO_WALL_UPPER_BOUND || dist90.distance < DISTANCE_TO_WALL_LOWER_BOUND) { + println("Flaw in distance to the parallel wall found, correcting") return correctDistanceToParallelWall(anglesDistances, state) } // default case: everything is ok, just move forward - val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) - resultBuilder.setDirections(getIntArray(FORWARD)) - resultBuilder.setDistances(getIntArray(20)) - return resultBuilder.build() + println("Default case: moving forward") + return moveForward() } private fun calculateAngle(anglesDistances: Map, state: CarState): Angle { diff --git a/server/src/main/java/algorithm/RoomModel.kt b/server/src/main/java/algorithm/RoomModel.kt index 693e739856b..ee69a26c1f7 100644 --- a/server/src/main/java/algorithm/RoomModel.kt +++ b/server/src/main/java/algorithm/RoomModel.kt @@ -3,6 +3,7 @@ package algorithm import DebugClInterface import DebugResponse import Waypoints +import algorithm.geometry.Angle import algorithm.geometry.Line import algorithm.geometry.Point import algorithm.geometry.Wall @@ -10,9 +11,7 @@ import objects.Car import java.util.* object RoomModel { - - val lines = arrayListOf() - val walls = arrayListOf(Wall()) + val walls = arrayListOf(Wall(Angle(0))) var finished = false val linesModel = listOf(Line(0.0, 1.0, -300.0), @@ -61,33 +60,49 @@ object RoomModel { val line2 = walls[walls.size - 2].line val intersection: Point = line1.intersect(line2) - walls[walls.size - 2].addPoint(intersection) + + + val lastWall = walls[walls.size - 1] + lastWall.pushFrontPoint(intersection) + if (lastWall.isHorizontal()) { + lastWall.line = Line(0.0, 1.0, -intersection.y) + } else if (lastWall.isVertical()) { + lastWall.line = Line(1.0, 0.0, -intersection.x) + } + + walls[walls.size - 2].pushBackPoint(intersection) + walls[walls.size - 2].markAsFinished() } fun getDebugInfo(): DebugResponse { - val aRef = IntArray(linesModel.size) - val bRef = IntArray(linesModel.size) - val cRef = IntArray(linesModel.size) - val a = IntArray(lines.size) - val b = IntArray(lines.size) - val c = IntArray(lines.size) + val points = getWallsPoints() + val begin_x = IntArray(points.size) + val begin_y = IntArray(points.size) + val end_x = IntArray(points.size) + val end_y = IntArray(points.size) + + val rawPoints = walls.flatMap({ it.rawPoints }).toTypedArray() + val pointsX = rawPoints.map { it.x.toInt() }.toIntArray() + val pointsY = rawPoints.map { it.y.toInt() }.toIntArray() + + for (i in 0..points.size - 2) { + val curPoint = points[i] + val nextPoint = points[i + 1] + begin_x[i] = (curPoint.x + 0.5).toInt() + begin_y[i] = (curPoint.y + 0.5).toInt() + + end_x[i] = (nextPoint.x + 0.5).toInt() + end_y[i] = (nextPoint.y + 0.5).toInt() + } val alg = DebugClInterface.algorithmImpl - val car = if (alg == null) Car(0, "", 0) else alg.thisCar - val mult = 100 - linesModel.forEachIndexed { idx, line -> - aRef[idx] = (line.A * mult).toInt() - bRef[idx] = (line.B * mult).toInt() - cRef[idx] = (line.C * mult).toInt() - } - lines.forEachIndexed { idx, line -> - a[idx] = (line.A * mult).toInt() - b[idx] = (line.B * mult).toInt() - c[idx] = (line.C * mult).toInt() + if (alg is RoomBypassingAlgorithm) { + return DebugResponse.BuilderDebugResponse(begin_x, begin_y, end_x, end_y, alg.carX, + alg.carY, alg.carAngle, pointsX, pointsY).build() } + return DebugResponse.BuilderDebugResponse(begin_x, begin_y, end_x, end_y, 0, + 0, 0, pointsX, pointsY).build() - val resultBuilder = DebugResponse.BuilderDebugResponse(a, b, c, car.x, car.y, car.angle, aRef, bRef, cRef) - return resultBuilder.build() } } \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Angle.kt b/server/src/main/java/algorithm/geometry/Angle.kt index e8a2544a479..72083c1862e 100644 --- a/server/src/main/java/algorithm/geometry/Angle.kt +++ b/server/src/main/java/algorithm/geometry/Angle.kt @@ -16,6 +16,10 @@ data class Angle(private val degrees: Int) { return Angle(degrees - other.degrees) } + operator fun unaryMinus(): Angle { + return Angle(-degrees) + } + operator fun plus(other: Angle): Angle { return Angle(degrees + other.degrees) } diff --git a/server/src/main/java/algorithm/geometry/Line.kt b/server/src/main/java/algorithm/geometry/Line.kt index c839861b262..815e4dcf360 100644 --- a/server/src/main/java/algorithm/geometry/Line.kt +++ b/server/src/main/java/algorithm/geometry/Line.kt @@ -1,7 +1,7 @@ package algorithm.geometry class Line(var A: Double, var B: Double, var C: Double) { - val COMPARISON_THRESHOLD = 0.5 + val COMPARISON_THRESHOLD = 20 init { normalize() @@ -55,4 +55,9 @@ class Line(var A: Double, var B: Double, var C: Double) { return metricDist(other) < COMPARISON_THRESHOLD } + + fun getDirectionVector(): Vector { + return Vector (A, -B) + } + } \ 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 index 2f3f5708ea0..10dc6ada1f0 100644 --- a/server/src/main/java/algorithm/geometry/Point.kt +++ b/server/src/main/java/algorithm/geometry/Point.kt @@ -1,6 +1,7 @@ package algorithm.geometry data class Point(val x: Double, val y: Double) { - - + operator fun plus(other: Vector): Point { + return Point(x + other.x, y + other.y) + } } \ 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 index 9e13be4b5ea..8f08eedb065 100644 --- a/server/src/main/java/algorithm/geometry/Util.kt +++ b/server/src/main/java/algorithm/geometry/Util.kt @@ -2,5 +2,5 @@ package algorithm.geometry object Util { - val eps = 1e-2 + val eps = 0.1 } \ No newline at end of file diff --git a/server/src/main/java/algorithm/geometry/Vector.kt b/server/src/main/java/algorithm/geometry/Vector.kt index 4759fa1ce6a..46e654f56fa 100644 --- a/server/src/main/java/algorithm/geometry/Vector.kt +++ b/server/src/main/java/algorithm/geometry/Vector.kt @@ -1,9 +1,10 @@ package algorithm.geometry -class Vector constructor(val x: Double, val y: Double) { +class Vector constructor(var x: Double, var y: Double) { constructor(x1: Double, y1: Double, x2: Double, y2: Double) : this(x2 - x1, y2 - y1) + constructor(begin: Point, end: Point) : this (begin.x, begin.y, end.x, end.y) fun scalarProduct(vector: Vector): Double { return this.x * vector.x + this.y * vector.y @@ -13,4 +14,21 @@ class Vector constructor(val x: Double, val y: Double) { return Math.sqrt(x * x + y * y) } + fun angleTo(other: Vector): Angle { + var sp = scalarProduct(other) + var cosinus = sp / length() / other.length() + return Angle(Math.acos(cosinus)) + } + + fun inverse() { + x *= -1 + y *= -1 + } + + fun normalize() { + val div = Math.sqrt(x * x + y * y) + + x /= div + y /= div + } } diff --git a/server/src/main/java/algorithm/geometry/Wall.kt b/server/src/main/java/algorithm/geometry/Wall.kt index 6bd1110b3f4..ba9b0cbc0a9 100644 --- a/server/src/main/java/algorithm/geometry/Wall.kt +++ b/server/src/main/java/algorithm/geometry/Wall.kt @@ -1,34 +1,108 @@ package algorithm.geometry import java.util.* +import algorithm.geometry.Vector -data class Wall(val points: ArrayList = arrayListOf(), +data class Wall(val wallAngleOX: Angle, + val rawPoints: ArrayList = arrayListOf(), var line: Line = Line(0.0, 0.0, 0.0)) { + var isFinished = false + val MAX_REGRESSION_ERROR = 10.0 - fun addPoint(point: Point) { - points.add(point) + fun pushBackPoint(point: Point) { + rawPoints.add(point) line = approximatePointsByLine() } + fun pushFrontPoint(point: Point) { + rawPoints.add(0, point) + line = approximatePointsByLine() + } + + var points: ArrayList = arrayListOf() + + fun markAsFinished() { + isFinished = true + points = generatePoints() + } + + private fun generatePoints(): ArrayList { + val direction = Vector(rawPoints.first(), rawPoints.last()) + val res: ArrayList = arrayListOf() + + val pointsCount = direction.length().toInt() + + direction.normalize() + + var curPoint = rawPoints.first() + res.add(curPoint) + val endPoint = rawPoints.last() + for (i in 0..pointsCount - 1) { + curPoint += direction + res.add(curPoint) + } + res.add(endPoint) + + return res + } + private fun approximatePointsByLine(): Line { + var n = 0 - 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 } + var sumx = 0.0 + var sumy = 0.0 + var sumx2 = 0.0 + for (pt in rawPoints) { + sumx += pt.x + sumx2 += pt.x * pt.x + sumy += pt.y + n++ + } + val xbar = sumx / n + val ybar = sumy / n - val pointsCount = points.size - val den = pointsCount * sumXQuad - sumX * sumX - - if (Math.abs(den) < 0.001) { - return Line(1.0, 0.0, -points.first().x) + // second pass: compute summary statistics + var xxbar = 0.0 + var yybar = 0.0 + var xybar = 0.0 + for (pt in rawPoints) { + xxbar += (pt.x - xbar) * (pt.x - xbar) + yybar += (pt.y - ybar) * (pt.y - ybar) + xybar += (pt.x - xbar) * (pt.y - ybar) } - val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX) - val b = (sumY - sumX * k) / pointsCount + var beta1 = xybar / xxbar + var beta0 = ybar - beta1 * xbar - return Line(-k, 1.0, -b) + // analyze results + val df = n - 2 + var rss = 0.0 // residual sum of squares + var ssr = 0.0 // regression sum of squares + for (pt in rawPoints) { + val fit = beta1 * pt.x + beta0 + rss += (fit - pt.y) * (fit - pt.y) + ssr += (fit - ybar) * (fit - ybar) + } + + val svar = rss / df + val svar1 = svar / xxbar + + if (svar1.isNaN() || svar1.isInfinite() || Math.sqrt(svar1).gt(MAX_REGRESSION_ERROR) || Math.abs(beta1) > 1) { + beta1 = xybar / yybar + beta0 = xbar - beta1 * ybar + return Line(1.0, -beta1, -beta0) + } + + return Line(-beta1, 1.0, -beta0) + } + + fun isVertical(): Boolean { + return Math.abs(wallAngleOX.degs()) % 180 == 90 + } + + fun isHorizontal(): Boolean { + return Math.abs(wallAngleOX.degs()) % 180 == 0 } override fun equals(other: Any?): Boolean {