diff --git a/car_srv/kotlinSrv/src/CarState.kt b/car_srv/kotlinSrv/src/CarState.kt deleted file mode 100644 index d08bb87a879..00000000000 --- a/car_srv/kotlinSrv/src/CarState.kt +++ /dev/null @@ -1,33 +0,0 @@ -class CarState private constructor() { - //position - var x: Int - var y: Int - var angle: Int//positive is from OX to OY - - init { - this.x = 0 - this.y = 0 - this.angle = 0 - } - - //if distance is positive - move forward, else backward - fun moving(distance: Int) { - x += (Math.cos(degreesToRadian(angle)) * distance).toInt() - y += (Math.sin(degreesToRadian(angle)) * distance).toInt() - println("new position: {$x, $y}, distance: {$distance}") - } - - private fun degreesToRadian(angleInDegrees: Int): Double { - return Math.PI * angleInDegrees / 180 - } - - //angle positive - rotation left - fun rotate(angle: Int) { - this.angle = (this.angle + angle) % 360 - println("new angle: {$angle, ${this.angle}}") - } - - companion object { - val instance = CarState() - } -} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/Main.kt b/car_srv/kotlinSrv/src/Main.kt index 3db6853dcb5..b88575cc5ff 100644 --- a/car_srv/kotlinSrv/src/Main.kt +++ b/car_srv/kotlinSrv/src/Main.kt @@ -1,6 +1,4 @@ import control.car.ControllerToUsb -import control.emulator.ControllerEmulator -import control.emulator.Rng import net.Client import net.server.handlers.AbstractHandler import net.server.handlers.debug.Memory @@ -8,7 +6,6 @@ import net.server.handlers.debug.Sonar import net.server.handlers.flash.LoadBin import net.server.handlers.main.* import net.server.handlers.rc.Control -import room.Room val carServerPort: Int = 8888 val mainServerPort = 7925 @@ -21,48 +18,8 @@ fun main(args: Array) { println("incorrect config format!") js("process.exit(1)") } - val clArgs = js("process.argv") - var runAsEmulator = false - var runTests = false - var nextSeed = false - var room: Room = Room.testRoom1() - for (arg in clArgs) { - if (nextSeed) { - Rng.SEED = parseInt(arg, 10).toLong() - Rng.curState = Rng.SEED.toLong() - nextSeed = false - } - when (arg) { - "-t" -> runTests = true - "-e" -> runAsEmulator = true - "-tr1" -> room = Room.testRoom1() - "-tr2" -> room = Room.testRoom2() - "-tr3" -> room = Room.testRoom3() - "-r" -> Rng.ADD_RANDOM = true - "-s" -> nextSeed = true - } - if (arg.toString().contains("--room=")) { - val pathToFile = arg.toString().split("=")[1].replace("\"", "") - val data: String = fs.readFileSync(pathToFile, "utf8") - val roomOrNull = Room.roomFromString(data) - if (roomOrNull == null) { - println("error parsing string $data \nto room") - return - } - room = roomOrNull - } - } - val carController = - if (runAsEmulator) { - McState.instance.connect("") - ControllerEmulator(room) - } else { - ControllerToUsb() - } - if (runTests) { - runTests() - } + val carController = ControllerToUsb() val handlers = mutableMapOf() handlers.put("/rc/control", Control()) @@ -71,7 +28,6 @@ fun main(args: Array) { handlers.put("/sonarExplore", SonarExplore(carController)) handlers.put("/route", SetRoute(carController)) handlers.put("/routeMetric", SetRouteMetric(carController)) - handlers.put("/getLocation", GetLocation()) handlers.put("/debug/memory", Memory()) handlers.put("/debug/sonar", Sonar()) diff --git a/car_srv/kotlinSrv/src/Test.kt b/car_srv/kotlinSrv/src/Test.kt deleted file mode 100644 index 62b5bc60b76..00000000000 --- a/car_srv/kotlinSrv/src/Test.kt +++ /dev/null @@ -1,82 +0,0 @@ -import control.emulator.ControllerEmulator -import room.Room - -fun runTests() { - testCarEmulator() -} - -private fun testCarEmulator() { - val controller = ControllerEmulator(Room.testRoom1()) - var sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(2, { x -> x * 90 }), IntArray(0), 0, SonarRequest.Smoothing.NONE).build() - var sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() - controller.executeRequestSensorData(sonarRequest, { bytes -> - sonarResponse.mergeFrom(CodedInputStream(bytes)) - val dist0 = sonarResponse.distances[0] - val dist90 = sonarResponse.distances[1] - eq(dist0.toDouble(), 200.0, "check angle 0", 2.5) - eq(dist90.toDouble(), 20.0, "check angle 90", 2.5) - }) - - println("--------------------") - sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(2, { 120 + 60 * it }), IntArray(0), 0, SonarRequest.Smoothing.NONE).build() - sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() - controller.executeRequestSensorData(sonarRequest, { bytes -> - sonarResponse.mergeFrom(CodedInputStream(bytes)) - val dist120 = sonarResponse.distances[0] - val dist180 = sonarResponse.distances[1] - eq(dist120.toDouble(), 23.094, "check angle 120", 2.5) - eq(dist180.toDouble(), 150.0, "check angle 180", 2.5) - }) - - println("--------------------") - CarState.instance.x = 50 - CarState.instance.y = 50 - CarState.instance.angle = 90 - sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(3, { x -> x * 90 }), IntArray(0), 0, SonarRequest.Smoothing.NONE).build() - sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() - controller.executeRequestSensorData(sonarRequest, { bytes -> - sonarResponse.mergeFrom(CodedInputStream(bytes)) - val dist0 = sonarResponse.distances[0] - val dist90 = sonarResponse.distances[1] - val dist180 = sonarResponse.distances[2] - eq(dist0.toDouble(), 250.0, "check angle 0", 2.5) - eq(dist90.toDouble(), 150.0, "check angle 90", 2.5) - eq(dist180.toDouble(), 70.0, "check angle 180", 2.5) - }) - - println("--------------------") - CarState.instance.x = 50 - CarState.instance.y = 50 - CarState.instance.angle = 720 + 45 - sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(1, { x -> 90 }), IntArray(0), 0, SonarRequest.Smoothing.NONE).build() - sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() - controller.executeRequestSensorData(sonarRequest, { bytes -> - sonarResponse.mergeFrom(CodedInputStream(bytes)) - val dist90 = sonarResponse.distances[0] - eq(dist90.toDouble(), 98.9949, "check angle 90", 2.5) - }) - - println("--------------------") - CarState.instance.x = 150 - CarState.instance.y = 50 - CarState.instance.angle = 0 - sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(7, { it * 5 }), IntArray(0), 0, SonarRequest.Smoothing.NONE).build() - sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() - controller.executeRequestSensorData(sonarRequest, { bytes -> - sonarResponse.mergeFrom(CodedInputStream(bytes)) - val dist30 = sonarResponse.distances[7] - eq(dist30.toDouble(), 98.9949, "check angle 30", 2.5) - }) -} - -private var testNum = 1 - -private fun eq(value1: Double, value2: Double, msg: String, eps: Double) { - if (Math.abs(value1 - value2) > eps) { - println("test failed! $msg") - println("returned: $value1") - println("waited: $value2") - } else { - println("test ${testNum++} ok!") - } -} diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt deleted file mode 100644 index abd20fcc4c0..00000000000 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ /dev/null @@ -1,191 +0,0 @@ -package control.emulator - -import CarState -import RouteMetricRequest -import RouteRequest -import RouteResponse -import SonarExploreAngleRequest -import SonarRequest -import SonarResponse -import control.Controller -import encodeProtoBuf -import geometry.Line -import geometry.Vector -import room.Room -import kotlin.Pair - -class ControllerEmulator(val room: Room) : Controller { - private val MOVE_VELOCITY = 0.05//sm/ms - private val ROTATION_VELOCITY = 0.05//degrees/ms - - enum class MoveDirection { - LEFT, - RIGHT, - FORWARD, - BACKWARD, - ERROR - } - - override fun executeRoute(route: RouteRequest, callback: (ByteArray) -> Unit) { - val moveTimes = route.times - val moveDirections = route.directions - //list of move direction and time to this move in ms - val commands: MutableList> = mutableListOf() - - moveTimes.forEachIndexed { idx, value -> - val moveDirection = - when (moveDirections[idx]) { - 0 -> MoveDirection.FORWARD - 1 -> MoveDirection.BACKWARD - 2 -> MoveDirection.LEFT - 3 -> MoveDirection.RIGHT - else -> MoveDirection.ERROR - } - - if (moveDirection == MoveDirection.FORWARD || moveDirection == MoveDirection.BACKWARD) { - commands.add(Pair(moveDirection, value * MOVE_VELOCITY)) - } else { - commands.add(Pair(moveDirection, value * ROTATION_VELOCITY)) - } - } - executeCommand(commands, 0, callback) - } - - override fun executeMetricRoute(request: RouteMetricRequest, callback: (ByteArray) -> Unit) { - val commands: MutableList> = mutableListOf() - - val moveDistances = request.distances - moveDistances.forEachIndexed { idx, value -> - val moveDirection = - when (request.directions[idx]) { - 0 -> MoveDirection.FORWARD - 1 -> MoveDirection.BACKWARD - 2 -> MoveDirection.LEFT - 3 -> MoveDirection.RIGHT - else -> MoveDirection.ERROR - } - commands.add(Pair(moveDirection, value.toDouble())) - } - executeCommand(commands, 0, callback) - } - - override fun executeRequestSensorExploreData(request: SonarExploreAngleRequest, callback: (ByteArray) -> Unit) { - throw UnsupportedOperationException("not implemented") //To change body of created functions use File | Settings | File Templates. - } - - override fun executeRequestSensorData(sonarRequest: SonarRequest, callback: (ByteArray) -> Unit) { - val angles = sonarRequest.angles - val xSensor0 = CarState.instance.x - val ySensor0 = CarState.instance.y - val carAngle = CarState.instance.angle - - val distances = arrayListOf() - angles.forEach { angle -> - val angleFinal = getSensorAngle(angle, carAngle) - val xSensor1: Int - val ySensor1: Double - //tg can be equal to inf if angle = 90 or 270. it vertical line. x1 = x0, y1 = y0+-[any number. eg 1] - when (angleFinal) { - 90 -> { - xSensor1 = xSensor0 - ySensor1 = (ySensor0 + 1).toDouble() - } - 270 -> { - xSensor1 = xSensor0 - ySensor1 = (ySensor0 - 1).toDouble() - } - in (90..270) -> { - xSensor1 = xSensor0 - 1 - ySensor1 = ySensor0 + (xSensor1 - xSensor0) * Math.tan(angleFinal * Math.PI / 180) - } - else -> { - xSensor1 = xSensor0 + 1 - ySensor1 = ySensor0 + (xSensor1 - xSensor0) * Math.tan(angleFinal * Math.PI / 180) - } - } - val sensorLine = Line(ySensor0 - ySensor1, xSensor1.toDouble() - xSensor0, - xSensor0 * ySensor1 - ySensor0 * xSensor1) - - val sensorVector = Vector(xSensor0.toDouble(), ySensor0.toDouble(), xSensor1.toDouble(), ySensor1) - val distance = getDistance(xSensor0, ySensor0, sensorLine, sensorVector) - if (distance == -1) { - distances.add(distance) - } else { - val delta = if (Rng.ADD_RANDOM) Rng.nextInt(-2, 2) else 0//return one of -2 -1 0 1 or 2 - distances.add(distance + delta) - } - } - val responseMessage = SonarResponse.BuilderSonarResponse(distances.toIntArray()).build() - val bytesMessage = encodeProtoBuf(responseMessage) - callback.invoke(bytesMessage) - } - - private fun getDistance(xSensor0: Int, ySensor0: Int, sensorLine: Line, sensorVector: Vector): Int { - var result = Int.MAX_VALUE - for (wall in room.walls) { - val wallLine = wall.line - val slope = sensorLine.A * wallLine.B - sensorLine.B * wallLine.A - if (Math.abs(slope) < 0.01) { - //line is parallel. - continue - } - val xIntersection = (sensorLine.B * wallLine.C - wallLine.B * sensorLine.C) / slope - val yIntersection = (sensorLine.C * wallLine.A - wallLine.C * sensorLine.A) / slope - - //filters by direction and intersection position - val intersectionVector = Vector(xSensor0.toDouble(), ySensor0.toDouble(), xIntersection, yIntersection) - if (intersectionVector.scalarProduct(sensorVector) < 0) { - continue - } - val wallVector1 = Vector(xIntersection, yIntersection, wall.xTo.toDouble(), wall.yTo.toDouble()) - val wallVector2 = Vector(xIntersection, yIntersection, wall.xFrom.toDouble(), wall.yFrom.toDouble()) - if (wallVector1.scalarProduct(wallVector2) > 0) { - continue - } - val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0) - + Math.pow(yIntersection - ySensor0, 2.0))) - if (currentDistance < result) { - result = currentDistance - } - } - return result - } - - //return sensor angle in [0, 360) with considering car rotation - private fun getSensorAngle(requestAngle: Int, carAngle: Int): Int { - var angleTmp = carAngle - requestAngle - while (angleTmp < 0) { - angleTmp += 360 - } - return angleTmp % 360 - } - - fun executeCommand(commands: List>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) { - if (currentCommandIdx == commands.size) { - val responseMessage = RouteResponse.BuilderRouteResponse(0).build() - callBack.invoke(encodeProtoBuf(responseMessage)) - return - } - val currentCommand = commands[currentCommandIdx] - - //refresh car state - val carInstance = CarState.instance - val commandDistance = currentCommand.second - val coef = if (Rng.ADD_RANDOM) Rng.nextInt(900, 1100).toDouble() / 1000.0 else 1.0 - val commandDistanceIncludeRandom = (commandDistance * coef).toInt() - when (currentCommand.first) { - MoveDirection.FORWARD -> carInstance.moving(commandDistanceIncludeRandom) - MoveDirection.BACKWARD -> carInstance.moving(commandDistanceIncludeRandom) - MoveDirection.RIGHT -> carInstance.rotate(-commandDistanceIncludeRandom) - MoveDirection.LEFT -> carInstance.rotate(commandDistanceIncludeRandom) - else -> { - } - } - -// setTimeout({ - executeCommand(commands, currentCommandIdx + 1, callBack) -// }, currentCommand.second) - } - - -} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/control/emulator/Rng.kt b/car_srv/kotlinSrv/src/control/emulator/Rng.kt deleted file mode 100644 index 0d88af4f150..00000000000 --- a/car_srv/kotlinSrv/src/control/emulator/Rng.kt +++ /dev/null @@ -1,31 +0,0 @@ -package control.emulator - -object Rng { - var SEED = 55L - var curState = SEED - val a = 1664525L - val c = 1013904223L - val mod = 2147483648L - - var ADD_RANDOM = false - - fun abs(value: Long): Long { - if (value < 0) - return -value - return value - } - - fun nextInt(): Int { - val res = curState - curState = abs(a * curState + c) % mod - return (res % mod).toInt() - } - - fun nextInt(min_val: Int, max_val: Int): Int { - val rand = nextInt() - val size = max_val - min_val + 1 - val randInRange = rand % size - val res = randInRange + min_val - return res - } -} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/geometry/Line.kt b/car_srv/kotlinSrv/src/geometry/Line.kt deleted file mode 100644 index 7767e6e269b..00000000000 --- a/car_srv/kotlinSrv/src/geometry/Line.kt +++ /dev/null @@ -1,4 +0,0 @@ -package geometry - -class Line(val A: Double, val B: Double, val C: Double) { -} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/geometry/Vector.kt b/car_srv/kotlinSrv/src/geometry/Vector.kt deleted file mode 100644 index 676d6ce8f3d..00000000000 --- a/car_srv/kotlinSrv/src/geometry/Vector.kt +++ /dev/null @@ -1,12 +0,0 @@ -package geometry - -class Vector constructor(val x: Double, val y: Double) { - - constructor(x1: Double, y1: Double, x2: Double, y2: Double) : this(x2 - x1, y2 - y1) - - - fun scalarProduct(vector: Vector): Double { - return this.x * vector.x + this.y * vector.y - } - -} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/net/server/handlers/main/GetLocation.kt b/car_srv/kotlinSrv/src/net/server/handlers/main/GetLocation.kt deleted file mode 100644 index 606c8ca6820..00000000000 --- a/car_srv/kotlinSrv/src/net/server/handlers/main/GetLocation.kt +++ /dev/null @@ -1,23 +0,0 @@ -package net.server.handlers.main - -import CarState -import LocationResponse -import encodeProtoBuf -import net.server.handlers.AbstractHandler - -class GetLocation : AbstractHandler { - - val toServerObjectBuilder: LocationResponse.BuilderLocationResponse - - constructor() : super() { - val defaultLocationData = LocationResponse.LocationData.BuilderLocationData(0, 0, 0).build() - this.toServerObjectBuilder = LocationResponse.BuilderLocationResponse(defaultLocationData, 0) - } - - override fun getBytesResponse(data: ByteArray, callback: (ByteArray) -> Unit) { - val carState = CarState.instance - val locationData = LocationResponse.LocationData.BuilderLocationData(carState.x.toInt(), carState.y.toInt(), carState.angle.toInt()).build() - val responseMessage = toServerObjectBuilder.setLocationResponseData(locationData).build() - callback.invoke(encodeProtoBuf(responseMessage)) - } -} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/room/Room.kt b/car_srv/kotlinSrv/src/room/Room.kt deleted file mode 100644 index 80176964ce3..00000000000 --- a/car_srv/kotlinSrv/src/room/Room.kt +++ /dev/null @@ -1,91 +0,0 @@ -package room - -import geometry.Line -import require - -class Room() { - - val walls = arrayListOf() - - constructor(walls: Collection) : this() { - this.walls.addAll(walls) - } - - companion object { - fun roomFromString(roomString: String): Room? { - val xmlParser = require("xml-parser") - val xmlObject = xmlParser(roomString) - val wallsArray: dynamic - try { - wallsArray = xmlObject.root.children[0].children - } catch (e: Exception) { - return null - } - val result = Room() - for (i in 0..(parseInt(wallsArray.length) - 1)) { - val wall = Wall.wallFromXml(wallsArray[i]) ?: return null - result.walls.add(wall) - } - return result - } - - fun testRoom1(): Room { - val upLine = Line(0.0, 1.0, -300.0) - val leftLine = Line(1.0, 0.0, 150.0) - val bottomLine = Line(0.0, 1.0, 45.0) - val rightLine = Line(1.0, 0.0, -200.0) - val walls = listOf(Wall(upLine, 200, -150, 300, 300), - Wall(leftLine, -150, -150, 300, -45), - Wall(bottomLine, -150, 200, -45, -45), - Wall(rightLine, 200, 200, -45, 300) - ) - return Room(walls) - } - - fun testRoom2(): Room { - - val upLine = Line(0.0, 1.0, -300.0) - val leftLine = Line(1.0, 0.0, 150.0) - val bottomLine = Line(0.0, 1.0, 45.0) - val rightLine = Line(1.0, 0.0, -200.0) - - val bottomLine2 = Line(0.0, 1.0, -100.0) - val rightLine2 = Line(1.0, 0.0, -300.0) - - val walls = listOf(Wall(upLine, 300, -150, 300, 300), - Wall(leftLine, -150, -150, 300, -45), - Wall(bottomLine, -150, 200, -45, -45), - Wall(rightLine, 200, 200, -45, 100), - Wall(bottomLine2, 200, 300, 100, 100), - Wall(rightLine2, 300, 300, 100, 300) - ) - return Room(walls) - } - - fun testRoom3(): Room { - - val upLine = Line(0.0, 1.0, -300.0) - val leftLine = Line(1.0, 0.0, 150.0) - val bottomLine = Line(0.0, 1.0, 20.0) - val rightLine = Line(-1.2, 1.0, 140.0) - val rightLine2 = Line(1.0, 0.0, -200.0) - - val walls = listOf(Wall(upLine, 200, -150, 300, 300), - Wall(leftLine, -150, -150, 300, -20), - Wall(bottomLine, -150, 100, -20, -20), - Wall(rightLine, 100, 200, -20, 100), - Wall(rightLine2, 200, 200, 100, 300) - ) - - return Room(walls) - } - } - - fun roomToString(): String { - var result = "" - walls.forEach { - result += it.wallToString() + "\n" - } - return result - } -} diff --git a/car_srv/kotlinSrv/src/room/Wall.kt b/car_srv/kotlinSrv/src/room/Wall.kt deleted file mode 100644 index 812c616c3fa..00000000000 --- a/car_srv/kotlinSrv/src/room/Wall.kt +++ /dev/null @@ -1,30 +0,0 @@ -package room - -import geometry.Line - -class Wall constructor(val line: Line, val xFrom: Int, val xTo: Int, val yFrom: Int, val yTo: Int) { - - companion object { - fun wallFromXml(wall: dynamic): Wall? { - val points = wall.attributes - val startX: Int - val endX: Int - val endY: Int - val startY: Int - try { - startX = parseInt(points.startX) - startY = parseInt(points.startY) - endX = parseInt(points.endX) - endY = parseInt(points.endY) - } catch (e: NumberFormatException) { - return null - } - val line = Line((startY - endY).toDouble(), (endX - startX).toDouble(), (startX * endY - startY * endX).toDouble()) - return Wall(line, startX, endX, startY, endY) - } - } - - fun wallToString(): String { - return "xFrom: $xFrom, yFrom: $yFrom, xTo: $xTo, yTo: $yTo" - } -}