From bb1d91ac6235ad1c4cba7f112cd040aaa974a2b1 Mon Sep 17 00:00:00 2001 From: MaximZaitsev Date: Fri, 26 Aug 2016 17:55:41 +0300 Subject: [PATCH] make new algorithm. based on car position --- server/src/main/java/DebugClInterface.kt | 8 +- .../main/java/algorithm/AbstractAlgorithm.kt | 6 +- .../java/algorithm/RoomBypassingAlgorithm.kt | 28 +++++- server/src/main/java/algorithm/RoomModel.kt | 45 +++++---- server/src/main/java/algorithm/RoomTest.kt | 96 +++++++++++++++++++ 5 files changed, 161 insertions(+), 22 deletions(-) create mode 100644 server/src/main/java/algorithm/RoomTest.kt diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index 2b180c7a95f..81bdaedc41f 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -2,6 +2,7 @@ import Exceptions.InactiveCarException import algorithm.AbstractAlgorithm import algorithm.RoomBypassingAlgorithm import algorithm.RoomModel +import algorithm.RoomTest import car.client.Client import io.netty.buffer.Unpooled import io.netty.handler.codec.http.* @@ -64,6 +65,10 @@ object DebugClInterface { val tmp = algorithmImpl if (tmp is RoomBypassingAlgorithm) { println("length: ${tmp.wallLength} angleOX: ${tmp.wallAngleWithOX}") + println("x: ${tmp.carX} y: ${tmp.carY} angle:${tmp.carAngle}") + } else if (tmp is RoomTest) { + println("points: ${tmp.points}") + println("x: ${tmp.carX} y: ${tmp.carY} angle:${tmp.carAngle}") } } "alg" -> executeAlg(readString) @@ -81,7 +86,8 @@ object DebugClInterface { } else 1 if (algorithmImpl == null) { - algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger) + algorithmImpl = RoomTest(environment.map.values.last(), exchanger) +// algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger) } while (count > 0) { count-- diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index aef7933fb40..e1f49e07504 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -49,7 +49,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger): CarState? protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteRequest + protected abstract fun afterGetCommand(route:RouteRequest) private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest { diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index d67c0eb523e..6cd5b2316bb 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -16,6 +16,9 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs public var wallLength = 0.0//sm var errorCount = 0 + var carX = 0 + var carY = 0 + var carAngle = 0 private var prevPoint = Pair(0.0, 0.0) @@ -40,6 +43,27 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs return args } + override fun afterGetCommand(route: RouteRequest) { + route.directions.forEachIndexed { idx, direction -> + when (direction) { + FORWARD -> { + carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt() + carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt() + } + BACKWARD -> { + carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt() + carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt() + } + LEFT -> { + carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt() + } + RIGHT -> { + carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt() + } + } + } + } + override fun getCommand(anglesDistances: Map, state: CarState): RouteRequest { val dist0 = anglesDistances[0] val dist60 = anglesDistances[60] @@ -74,8 +98,8 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs if (dist90 > 40 || dist90 < 20) { val rotationDirection = if (dist90 > 40) RIGHT else LEFT resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) - resultBuilder.setTimes(getIntArray((30.0 / ROTATION_VELOCITY).toInt(), - (25.0 / MOVE_VELOCITY).toInt())) + resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(), + (35.0 / MOVE_VELOCITY).toInt())) return resultBuilder.build() } diff --git a/server/src/main/java/algorithm/RoomModel.kt b/server/src/main/java/algorithm/RoomModel.kt index 462040bcb33..936dc8b4f8f 100644 --- a/server/src/main/java/algorithm/RoomModel.kt +++ b/server/src/main/java/algorithm/RoomModel.kt @@ -1,8 +1,9 @@ package algorithm +import DebugClInterface +import DebugResponse import Waypoints import algorithm.geometry.Line -import DebugResponse import objects.Car object RoomModel { @@ -21,27 +22,37 @@ object RoomModel { var currentPosition_x = 0 var currentPosition_y = 0 fun getUpdate(): Waypoints { - if (iter == 5) { - web.server.Server.changeMode(web.server.Server.ServerMode.IDLE) - return Waypoints.BuilderWaypoints(IntArray(0), IntArray(0), IntArray(0), IntArray(0), true).build() + + val algorithm = DebugClInterface.algorithmImpl + if (algorithm == null || !(algorithm is RoomTest)) { + val emptyArr = IntArray(0) + return Waypoints.BuilderWaypoints(emptyArr, emptyArr, emptyArr, emptyArr, false).build() + } + val begin_x = IntArray(algorithm.points.size) + val begin_y = IntArray(algorithm.points.size) + val end_x = IntArray(algorithm.points.size) + val end_y = IntArray(algorithm.points.size) - val begin_x = IntArray(10) - val begin_y = IntArray(10) - val end_x = IntArray(10) - val end_y = IntArray(10) - for (i in 0..9) { - begin_x[i] = currentPosition_x - begin_y[i] = currentPosition_y + for (i in 0..algorithm.points.size - 1) { + 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() - currentPosition_x += rng.nextInt(3) - currentPosition_y += rng.nextInt(3) - - end_x[i] = currentPosition_x - end_y[i] = currentPosition_y + end_x[i] = nextPoint.first.toInt() + end_y[i] = nextPoint.second.toInt() } - iter++ 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 new file mode 100644 index 00000000000..8438f1cf536 --- /dev/null +++ b/server/src/main/java/algorithm/RoomTest.kt @@ -0,0 +1,96 @@ +package algorithm + +import objects.Car +import java.util.concurrent.Exchanger +import RouteRequest + +class RoomTest(thisCar: Car, exchanger: Exchanger) : AbstractAlgorithm(thisCar, exchanger) { + + private val MOVE_VELOCITY = 0.05//sm/ms + private val ROTATION_VELOCITY = 0.05//degrees/ms + + val points = arrayListOf>() + + + var carX = 0 + var carY = 0 + var carAngle = 0 + + override fun getCarState(anglesDistances: Map): CarState { + return CarState.WALL + } + + override fun getCommand(anglesDistances: Map, state: CarState): RouteRequest { + val dist0 = anglesDistances[0] + val dist60 = anglesDistances[60] + val dist90 = anglesDistances[90] + val dist120 = anglesDistances[120] + val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0)) + if (dist90 == null) { + resultBuilder.setDirections(getIntArray(RIGHT)) + resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt())) + return resultBuilder.build() + } + val sonarAngle = carAngle - 90 + points.add(Pair(carX + dist90 * Math.cos(degreesToRadian(sonarAngle)), + carY + dist90 * Math.sin(degreesToRadian(sonarAngle)))) + if (dist120 == null || dist60 == null) { + //hmm + resultBuilder.setDirections(getIntArray(FORWARD)) + resultBuilder.setTimes(getIntArray((10 / MOVE_VELOCITY).toInt())) + return resultBuilder.build() + } + if (dist0 != null && dist0 < 60) { + resultBuilder.setDirections(getIntArray(LEFT, FORWARD)) + resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt())) + return resultBuilder.build() + } + val abs = Math.abs(dist120 - dist60) + if (abs > 10) { + val rotationDirection = if (dist120 > dist60) LEFT else RIGHT + resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) + resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(), (7 / MOVE_VELOCITY).toInt()))//todo calibrate + return resultBuilder.build() + } + if (dist90 > 50 || dist90 < 25) { + val rotationDirection = if (dist90 > 50) RIGHT else LEFT + resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) + resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt())) + return resultBuilder.build() + } + + resultBuilder.setDirections(getIntArray(FORWARD)) + resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt())) + return resultBuilder.build() + } + + private fun getIntArray(vararg args: Int): IntArray { + return args + } + + override fun afterGetCommand(route: RouteRequest) { + route.directions.forEachIndexed { idx, direction -> + when (direction) { + FORWARD -> { + carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt() + carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt() + } + BACKWARD -> { + carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt() + carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt() + } + LEFT -> { + carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt() + } + RIGHT -> { + carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt() + } + } + } + } + + private fun degreesToRadian(angle: Int): Double { + return Math.PI * angle / 180 + } + +} \ No newline at end of file