From af1ee06ec6b3d88873bd0c1469bc753deffd688e Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Wed, 31 Aug 2016 12:23:50 +0300 Subject: [PATCH] working algorithm in box room --- server/.gitignore | 1 + server/src/main/java/DebugClInterface.kt | 1 + server/src/main/java/Logger.kt | 15 +++ .../main/java/algorithm/AbstractAlgorithm.kt | 12 +- .../java/algorithm/RoomBypassingAlgorithm.kt | 121 +++++++++++------- .../src/main/java/algorithm/geometry/Line.kt | 12 +- .../src/main/java/algorithm/geometry/Wall.kt | 13 +- 7 files changed, 127 insertions(+), 48 deletions(-) create mode 100644 server/src/main/java/Logger.kt diff --git a/server/.gitignore b/server/.gitignore index bfdb99e0887..04581cd4b4b 100644 --- a/server/.gitignore +++ b/server/.gitignore @@ -2,3 +2,4 @@ .gradle build out +*.log diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index 00ce61aace5..bbd591e8aef 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -44,6 +44,7 @@ object DebugClInterface { try { executeCommand(readString) } catch (exception: Exception) { + exception.printStackTrace() println("Fail to execute command[$readString]: $exception") } } diff --git a/server/src/main/java/Logger.kt b/server/src/main/java/Logger.kt new file mode 100644 index 00000000000..1c6baaf5802 --- /dev/null +++ b/server/src/main/java/Logger.kt @@ -0,0 +1,15 @@ +import java.io.BufferedWriter +import java.io.File +import java.io.FileOutputStream +import java.io.FileWriter + +object Logger { + val file = File("debug.log") + val fileWriter = FileWriter(file.absoluteFile) + val bufferedWriter = BufferedWriter(fileWriter) + + fun log(msg: String) { + bufferedWriter.write(msg + "\n") + bufferedWriter.flush() + } +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 1defeef11c4..68b109cfc41 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -30,7 +30,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() - private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(70), Angle(80), Angle(90), Angle(100), Angle(110), Angle(120), Angle(180)) + private val defaultAngles = arrayOf(Angle(0), Angle(70), Angle(75), Angle(80), Angle(85), Angle(90), Angle(95), Angle(100), Angle(105), Angle(110), 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 { @@ -39,6 +40,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger): IntArray { val anglesIntArray = (angles.map { it -> it.degs() }).toIntArray() @@ -93,6 +96,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { + // TODO: set to appropriate values override val ATTEMPTS: Int = 5 override val SMOOTHING = SonarRequest.Smoothing.MEDIAN override val WINDOW_SIZE = 3 + //SHOULD BE CALIBRATED BEFORE RUNNING!!!!!!!!!!! + private val CHARGE_CORRECTION = 0.85 + 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_MIN_DIFF = 5 // 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 @@ -61,15 +66,27 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs return resultBuilder.build() } - private fun alignParallelToWall(anglesDistances: Map, state: CarState): RouteMetricRequest { - val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + private fun tryAlignParallelToWall(anglesDistances: Map, state: CarState, average: Double): RouteMetricRequest? { + for (i in 0..15 step 5) { + val distRightForward = anglesDistances[Angle(70 + i)]!! + val distRightBackward = anglesDistances[Angle(110 - i)]!! + if (distRightBackward.distance > (average * 1.5) || distRightForward.distance > (average * 1.5)) { + continue + } - // we're sure that we have 120 and 60 degs measurements as they were checked before - val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT + if (Math.abs(distRightForward.distance - distRightBackward.distance) <= ISOSCALENESS_MIN_DIFF) { + return null + } - resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) - resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate - return resultBuilder.build() + log("Flaw in align found, correcting") + val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) + val rotationDirection = if (distRightBackward.distance > distRightForward.distance) LEFT else RIGHT + resultBuilder.setDirections(getIntArray(rotationDirection)) + resultBuilder.setDistances(getIntArray(Math.min(Math.abs(distRightBackward.distance - distRightForward.distance), 20))) + return resultBuilder.build() + } + + return null // TODO: everything is broken, what to do? } private fun correctDistanceToParallelWall(anglesDistances: Map, state: CarState): RouteMetricRequest { @@ -90,11 +107,13 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } private fun addWall(angleWithPrevWall: Angle) { + log("Adding wall") RoomModel.updateWalls() val firstWall = RoomModel.walls.first() val lastWall = RoomModel.walls.last() if (firstWall == lastWall && RoomModel.walls.size > 1) { + log("Found equal walls, finishing algorithm") RoomModel.finished = true lastWall.pushBackPoint(firstWall.rawPoints.last()) lastWall.markAsFinished() @@ -105,33 +124,38 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest? { - val dist0 = anglesDistances[Angle(0)]!! - val dist60 = anglesDistances[Angle(70)]!! + val dist70 = anglesDistances[Angle(70)]!! val dist90 = anglesDistances[Angle(90)]!! - val dist120 = anglesDistances[Angle(110)]!! + val dist110 = anglesDistances[Angle(110)]!! val sonarAngle = carAngle - 90 if (anglesDistances.filter { it.value.angle.degs() >= 60 && it.value.angle.degs() <= 120 && it.value.distance == -1 }.size != 0) { + log("Found -1 in angle distances, passing") return null } + val average = (anglesDistances.values + .filter { it.angle.degs() >= 60 && it.angle.degs() <= 120 } + .filter { it.distance != -1 } + .sumByDouble { it.distance.toDouble() }) / anglesDistances.values.size + log("Estimated average = $average") + 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(Math.min(Math.abs(dist60.distance - dist120.distance), 20))) - return resultBuilder.build() - } else { - calibrateAfterRotate = false - carAngle = RoomModel.walls.last().wallAngleOX.degs() + log("Calibrating after rotate") + val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average) + if (maybeAlignment != null) { + log("Realigning") + return maybeAlignment } + log("No need to realign") + calibrateAfterRotate = false + carAngle = RoomModel.walls.last().wallAngleOX.degs() } + // Check most basic measurements: 60/90/120 if (dist90.distance == -1 || dist90.distance > OUTER_CORNER_DISTANCE_THRESHOLD) { - println("No orthogonal measurement found") + log("No orthogonal measurement found") return noOrthogonalMeasurementFound(anglesDistances, state) } @@ -140,64 +164,70 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)), y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle)) ) + log("Adding ${point.toString()} to last wall") RoomModel.walls.last().pushBackPoint(point) // Check if corner reached if (dist0.distance != -1 && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) { - println("Wall ahead found") + log("Wall ahead found") return wallAheadFound(anglesDistances, state) } // Big fail, try to do something safe - if (dist120.distance == -1 && dist60.distance == -1) { - println("No parallel wall found") + if (dist110.distance == -1 && dist70.distance == -1) { + log("No parallel wall found") return noParallelWallFound(anglesDistances, state) } // Big fail too - if (dist120.distance == -1) { - println("No back measurement found, wtf") + if (dist110.distance == -1) { + log("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") + if (dist70.distance - dist0.distance > SPURIOUS_REFLECTION_DIFF) { + log("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") + if (dist70.distance == -1 || Math.abs(dist110.distance - dist70.distance) > ISOSCALENESS_MAX_DIFF) { + log("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) { + for (i in 70..110 step 10) { + if (i == 90) { + continue // point on 90 was already added + } + + val curDist = anglesDistances[Angle(i)]!!.distance + if (curDist > (average * 1.5)) { + continue + } 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)) + x = carX + curDist * Math.cos(degreesToRadian(carAngle - i)), + y = carY + curDist * 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) + // Try align parallel to wall + val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average) + if (maybeAlignment != null) { + log("Realigning") + return maybeAlignment } -// car stay parallel with wall -// carAngle = RoomModel.walls.last().wallAngleOX.degs() - // Check if wall is too close or too far 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") + log("Flaw in distance to the parallel wall found, correcting") return correctDistanceToParallelWall(anglesDistances, state) } // default case: everything is ok, just move forward - println("Default case: moving forward") + log("Default case: moving forward") return moveForward() } @@ -222,13 +252,16 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs carY -= (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt() } LEFT -> { + route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt() carAngle += route.distances[idx] } RIGHT -> { + route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt() carAngle -= route.distances[idx] } } } + } private fun degreesToRadian(angle: Int): Double { diff --git a/server/src/main/java/algorithm/geometry/Line.kt b/server/src/main/java/algorithm/geometry/Line.kt index 815e4dcf360..469d0d0ab3d 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 = 20 + val COMPARISON_THRESHOLD = 0 init { normalize() @@ -53,7 +53,15 @@ class Line(var A: Double, var B: Double, var C: Double) { if (!(other is Line)) return false - return metricDist(other) < COMPARISON_THRESHOLD + Logger.log("Comparing lines: ") + Logger.log(" this = ${this.toString()}") + Logger.log(" other = ${other.toString()}") + + val dist = metricDist(other) + + Logger.log(" Distance = ${dist}") + Logger.log(" COMPARISON_THRESHOLD = ${COMPARISON_THRESHOLD}") + return dist < COMPARISON_THRESHOLD } fun getDirectionVector(): Vector { diff --git a/server/src/main/java/algorithm/geometry/Wall.kt b/server/src/main/java/algorithm/geometry/Wall.kt index ba9b0cbc0a9..d5716a25146 100644 --- a/server/src/main/java/algorithm/geometry/Wall.kt +++ b/server/src/main/java/algorithm/geometry/Wall.kt @@ -5,7 +5,17 @@ import algorithm.geometry.Vector data class Wall(val wallAngleOX: Angle, val rawPoints: ArrayList = arrayListOf(), - var line: Line = Line(0.0, 0.0, 0.0)) { + var line: Line = Line(0.0, 0.0, 0.0) + ) { + val id: Int + companion object { + var idCounter = 0 + } + + init { + id = idCounter + idCounter++ + } var isFinished = false val MAX_REGRESSION_ERROR = 10.0 @@ -23,6 +33,7 @@ data class Wall(val wallAngleOX: Angle, var points: ArrayList = arrayListOf() fun markAsFinished() { + Logger.log("Marking wall with id = ${this.id} as finished") isFinished = true points = generatePoints() }