From e5ba0307a96128e3fa32186440c40bea48620fcf Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Thu, 1 Sep 2016 11:26:32 +0300 Subject: [PATCH] Improved condition of stopping algorithm; Changed error threshold in linear regression; --- server/src/main/java/DebugClInterface.kt | 2 +- .../main/java/algorithm/AbstractAlgorithm.kt | 2 +- .../main/java/algorithm/AlgorithmThread.kt | 4 +-- .../java/algorithm/RoomBypassingAlgorithm.kt | 36 +++++++++++++------ .../src/main/java/algorithm/geometry/Point.kt | 2 +- .../src/main/java/algorithm/geometry/Wall.kt | 5 +-- .../src/main/java/car/client/ClientHandler.kt | 2 -- 7 files changed, 32 insertions(+), 21 deletions(-) diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index bb52030737c..32a5a058a63 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -25,7 +25,7 @@ object DebugClInterface { "route [car_id] - setting a route for car with car id.\n" + "refloc - refresh all car locations\n" + "sonar [car_id] - get sonar data\n" + - "dbinfo [car_id] [type] - refresh all car locations\n." + + "dbinfo [car_id] [type] - refresh all car locations\n" + "lines - print lines, detected by car\n" + "alg [count] - run algorithm. Make [count] iteration\n" + "type is string name of value or int value. available values: MEMORYSTATS - 0\n" + diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 95f4d1b6ebe..8f54ea60f4e 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -77,7 +77,6 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger): CarState? protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest? protected abstract fun afterGetCommand(route: RouteMetricRequest) + abstract fun isCompleted(): Boolean private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest { diff --git a/server/src/main/java/algorithm/AlgorithmThread.kt b/server/src/main/java/algorithm/AlgorithmThread.kt index 460d90ffcf5..50198a5dfdd 100644 --- a/server/src/main/java/algorithm/AlgorithmThread.kt +++ b/server/src/main/java/algorithm/AlgorithmThread.kt @@ -1,13 +1,11 @@ package algorithm -import kotlin.concurrent.thread - class AlgorithmThread(val algorithmImpl: AbstractAlgorithm) : Thread() { private var count = 0 override fun run() { - while (true) {//todo while !alrightmImpl.isDone(), but need check it + while (!algorithmImpl.isCompleted()) { try { if (count > 0) { algorithmImpl.iterate() diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 927d694425e..9f5613a7931 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -3,10 +3,7 @@ package algorithm import Logger.log import RouteMetricRequest import SonarRequest -import algorithm.geometry.Angle -import algorithm.geometry.AngleData -import algorithm.geometry.Point -import algorithm.geometry.Wall +import algorithm.geometry.* import objects.Car import java.util.concurrent.Exchanger @@ -19,7 +16,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs override val WINDOW_SIZE = 3 //SHOULD BE CALIBRATED BEFORE RUNNING!!!!!!!!!!! - private val CHARGE_CORRECTION = 1.02//on full charge ok is 0.83 - 0.86 + private val CHARGE_CORRECTION = 0.88//on full charge ok is 0.83 - 0.86 private val MAX_DISTANCE_TO_WALL_AHEAD = 55 // reached the corner and should turn left private val OUTER_CORNER_DISTANCE_THRESHOLD = 90 // reached outer corner @@ -28,9 +25,13 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs private val DISTANCE_TO_WALL_UPPER_BOUND = 60 // have to move closer to the parallel wall private val DISTANCE_TO_WALL_LOWER_BOUND = 40 // 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 val RANGE_FROM_ZERO_POINT_TO_FINISH_ALG = 50 private var calibrateAfterRotate = false + private var isCompleted = false + private var circleFound = false + var carX = 0 var carY = 0 var carAngle = 0 @@ -50,8 +51,9 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) resultBuilder.setDirections(getIntArray(FORWARD, RIGHT, FORWARD)) val wallAngle = calculateAngle(anglesDistances, state) - resultBuilder.setDistances(getIntArray(15, wallAngle.degs(), 60)) + resultBuilder.setDistances(getIntArray(15, wallAngle.degs(), 70)) addWall(-wallAngle) + carAngle = RoomModel.walls.last().wallAngleOX.degs() calibrateAfterRotate = true return resultBuilder.build() } @@ -62,6 +64,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val wallAngle = calculateAngle(anglesDistances, state) resultBuilder.setDistances(getIntArray(wallAngle.degs(), 15)) addWall(wallAngle) + carAngle = RoomModel.walls.last().wallAngleOX.degs() calibrateAfterRotate = true return resultBuilder.build() } @@ -108,7 +111,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) resultBuilder.setDirections(getIntArray(FORWARD)) if (distToWall == -1 || distToWall > 200) { - resultBuilder.setDistances(getIntArray(50)) + resultBuilder.setDistances(getIntArray(70)) } else { resultBuilder.setDistances(getIntArray(Math.max(distToWall / 4, 20))) } @@ -121,10 +124,14 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val firstWall = RoomModel.walls.first() val lastWall = RoomModel.walls.last() - if (firstWall == lastWall && RoomModel.walls.size > 1) { - log("Found equal walls, finishing algorithm") +// if (firstWall == lastWall && RoomModel.walls.size > 1) { + if (circleFound) { + log("Found circle, finishing algorithm") + isCompleted = true RoomModel.finished = true - lastWall.pushBackPoint(firstWall.rawPoints.last()) + val intersectionPoint = firstWall.rawPoints.last() + intersectionPoint.y = lastWall.rawPoints.first().y + lastWall.pushBackPoint(intersectionPoint) lastWall.markAsFinished() RoomModel.walls.removeAt(0) } else { @@ -166,7 +173,6 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } log("No need to realign") calibrateAfterRotate = false - carAngle = RoomModel.walls.last().wallAngleOX.degs() } // Check most basic measurements: 60/90/120 @@ -256,6 +262,10 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs return args } + override fun isCompleted(): Boolean { + return isCompleted + } + override fun afterGetCommand(route: RouteMetricRequest) { route.directions.forEachIndexed { idx, direction -> when (direction) { @@ -277,6 +287,10 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } } } + if (Math.round(Math.cos(Angle(carAngle).rads())).toInt() == 1 && carAngle != 0 + && Vector(carX.toDouble(), carY.toDouble()).length() < RANGE_FROM_ZERO_POINT_TO_FINISH_ALG) { + circleFound = true + } } diff --git a/server/src/main/java/algorithm/geometry/Point.kt b/server/src/main/java/algorithm/geometry/Point.kt index 10dc6ada1f0..24181e9de37 100644 --- a/server/src/main/java/algorithm/geometry/Point.kt +++ b/server/src/main/java/algorithm/geometry/Point.kt @@ -1,6 +1,6 @@ package algorithm.geometry -data class Point(val x: Double, val y: Double) { +data class Point(var x: Double, var y: Double) { operator fun plus(other: Vector): Point { return Point(x + other.x, y + other.y) } diff --git a/server/src/main/java/algorithm/geometry/Wall.kt b/server/src/main/java/algorithm/geometry/Wall.kt index 07f4f1103fd..543e875be52 100644 --- a/server/src/main/java/algorithm/geometry/Wall.kt +++ b/server/src/main/java/algorithm/geometry/Wall.kt @@ -5,8 +5,9 @@ import java.util.* data class Wall(val wallAngleOX: Angle, val rawPoints: ArrayList = arrayListOf(), var line: Line = Line(0.0, 0.0, 0.0) - ) { +) { val id: Int + companion object { var idCounter = 0 } @@ -17,7 +18,7 @@ data class Wall(val wallAngleOX: Angle, } var isFinished = false - val MAX_REGRESSION_ERROR = 10.0 + val MAX_REGRESSION_ERROR = 5.0 fun pushBackPoint(point: Point) { rawPoints.add(point) diff --git a/server/src/main/java/car/client/ClientHandler.kt b/server/src/main/java/car/client/ClientHandler.kt index 99611cca83a..9144c28717f 100644 --- a/server/src/main/java/car/client/ClientHandler.kt +++ b/server/src/main/java/car/client/ClientHandler.kt @@ -97,8 +97,6 @@ class ClientHandler : SimpleChannelInboundHandler() { } catch (e: InterruptedException) { println("interrupted before sending datas to algorithm!") } catch (e: TimeoutException) { - e.printStackTrace() - println("timeout") } }