diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index bbd591e8aef..bb52030737c 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -1,5 +1,6 @@ import Exceptions.InactiveCarException import algorithm.AbstractAlgorithm +import algorithm.AlgorithmThread import algorithm.RoomBypassingAlgorithm import algorithm.RoomModel import car.client.CarClient @@ -15,6 +16,7 @@ object DebugClInterface { val exchanger = Exchanger() var algorithmImpl: AbstractAlgorithm? = null + var algorithmThread: AlgorithmThread? = null private val routeRegex = Regex("route [0-9]{1,10}") private val sonarRegex = Regex("sonar [0-9]{1,10}") @@ -101,20 +103,25 @@ object DebugClInterface { private fun executeAlg(readString: String) { val params = readString.split(" ") - var count = + val count = if (params.size == 2) try { params[1].toInt() } catch (e: Exception) { 1 } else 1 - if (algorithmImpl == null) { - algorithmImpl = RoomBypassingAlgorithm(Environment.map.values.last(), exchanger) + var alg = algorithmImpl + if (alg == null) { + alg = RoomBypassingAlgorithm(Environment.map.values.last(), exchanger) } - while (count > 0) { - count-- - algorithmImpl!!.iterate() + var algThread = algorithmThread + if (algThread == null) { + algThread = AlgorithmThread(alg) + algThread.start() } + algorithmImpl = alg + algThread.setCount(count) + algorithmThread = algThread } private fun executeDebugInfoCommand(readString: String) { diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 68b109cfc41..163c21eb0b9 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -15,6 +15,7 @@ import java.util.* import java.util.concurrent.Exchanger import java.util.concurrent.TimeUnit import java.util.concurrent.TimeoutException +import kotlin.concurrent.thread abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger) { @@ -63,8 +64,6 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger 0) { + algorithmImpl.iterate() + count-- + } else { + Thread.sleep(1000) + } + } catch (e: InterruptedException) { + + } + } + } + + fun setCount(count: Int) { + this.count = count + } + + fun cancel() { + count = 0 + } +} \ No newline at end of file diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 5e6f1861038..927d694425e 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -1,5 +1,6 @@ package algorithm +import Logger.log import RouteMetricRequest import SonarRequest import algorithm.geometry.Angle @@ -8,7 +9,6 @@ import algorithm.geometry.Point import algorithm.geometry.Wall import objects.Car import java.util.concurrent.Exchanger -import Logger.log class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { @@ -19,14 +19,14 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs override val WINDOW_SIZE = 3 //SHOULD BE CALIBRATED BEFORE RUNNING!!!!!!!!!!! - private val CHARGE_CORRECTION = 0.85 + private val CHARGE_CORRECTION = 1.02//on full charge ok is 0.83 - 0.86 - 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 MAX_DISTANCE_TO_WALL_AHEAD = 55 // reached the corner and should turn left + private val OUTER_CORNER_DISTANCE_THRESHOLD = 90 // reached outer corner 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 + 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 var calibrateAfterRotate = false @@ -70,7 +70,12 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs 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)) { + if (distRightBackward.distance == -1 || distRightForward.distance == -1) { + log("Found -1 in angle distances, passing") + continue + } + if (distRightBackward.distance > (average * 1.5) || distRightForward.distance > (average * 1.5) || + distRightBackward.distance < (average / 2) || distRightForward.distance < (average / 2)) { continue } @@ -93,16 +98,20 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) val rotationDirection = if (anglesDistances[Angle(90)]!!.distance > 50) RIGHT else LEFT - - resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) - resultBuilder.setDistances(getIntArray(10, 10)) + val backRotationDirection = if (rotationDirection == RIGHT) LEFT else RIGHT + resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD, backRotationDirection)) + resultBuilder.setDistances(getIntArray(15, 20, 5)) return resultBuilder.build() } - private fun moveForward(): RouteMetricRequest { + private fun moveForward(distToWall: Int): RouteMetricRequest { val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) resultBuilder.setDirections(getIntArray(FORWARD)) - resultBuilder.setDistances(getIntArray(20)) + if (distToWall == -1 || distToWall > 200) { + resultBuilder.setDistances(getIntArray(50)) + } else { + resultBuilder.setDistances(getIntArray(Math.max(distToWall / 4, 20))) + } return resultBuilder.build() } @@ -126,12 +135,19 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs override fun getCommand(anglesDistances: Map, state: CarState): RouteMetricRequest? { val dist0 = anglesDistances[Angle(0)]!! val dist70 = anglesDistances[Angle(70)]!! + val dist80 = anglesDistances[Angle(80)]!! val dist90 = anglesDistances[Angle(90)]!! + val dist100 = anglesDistances[Angle(100)]!! 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") + if (anglesDistances.filter { + it.value.angle.degs() >= 60 + && it.value.angle.degs() <= 120 + && it.value.distance == -1 + }.size > anglesDistances.size / 2) { + log("Found to many -1 in angle distances, passing") + //todo Теоретически такая ситуация может быть валидной, если сразу после внутреннего угла идёт внешний return null } @@ -182,20 +198,20 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs // Big fail too if (dist110.distance == -1) { log("No back measurement found, wtf") - return moveForward() + return moveForward(dist0.distance) } // Approaching inner corner and getting spurious reflection from 2 walls on 60 - just move forward to get closer to corner - 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 (dist70.distance == -1 || Math.abs(dist110.distance - dist70.distance) > ISOSCALENESS_MAX_DIFF) { - log("Aprroaching outer corner, moving forward") - return moveForward() - } +// 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 (dist80.distance == -1 || Math.abs(dist100.distance - dist80.distance) > ISOSCALENESS_MAX_DIFF) { +// log("Aprroaching outer corner, moving forward") +// return moveForward() +// } for (i in 70..110 step 10) { if (i == 90) { @@ -203,7 +219,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } val curDist = anglesDistances[Angle(i)]!!.distance - if (curDist > (average * 1.5)) { + if (curDist > (average * 1.5) || curDist < (average / 2)) { continue } val point = Point( @@ -228,7 +244,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs // default case: everything is ok, just move forward log("Default case: moving forward") - return moveForward() + return moveForward(dist0.distance) } private fun calculateAngle(anglesDistances: Map, state: CarState): Angle { @@ -252,12 +268,12 @@ 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] +// 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] +// route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt() +// carAngle -= route.distances[idx] } } } diff --git a/server/src/main/java/car/client/ClientHandler.kt b/server/src/main/java/car/client/ClientHandler.kt index 61c7e4171f2..99611cca83a 100644 --- a/server/src/main/java/car/client/ClientHandler.kt +++ b/server/src/main/java/car/client/ClientHandler.kt @@ -85,6 +85,13 @@ class ClientHandler : SimpleChannelInboundHandler() { private fun handlerSonarResponse(message: SonarResponse, angles: IntArray) { println("request angles: ${Arrays.toString(angles)}") println("distances from sonar: ${Arrays.toString(message.distances)}") + val algThread = DebugClInterface.algorithmThread + if (algThread == null) { + return + } + if (!algThread.isAlive) { + return + } try { DebugClInterface.exchanger.exchange(message.distances, 20, TimeUnit.SECONDS) } catch (e: InterruptedException) {