diff --git a/car_srv/kotlinSrv/src/JSImport.kt b/car_srv/kotlinSrv/src/JSImport.kt index f748c7d4093..44bf325d78d 100644 --- a/car_srv/kotlinSrv/src/JSImport.kt +++ b/car_srv/kotlinSrv/src/JSImport.kt @@ -22,7 +22,7 @@ fun encodeProtoBuf(protoMessage: T): ByteArray { routeBytes = ByteArray(protoSize) val codedOutput = CodedOutputStream(routeBytes) protoMessage.writeTo(codedOutput) - }else if (protoMessage is DebugRequest) { + } else if (protoMessage is DebugRequest) { val protoSize = protoMessage.getSizeNoTag() routeBytes = ByteArray(protoSize) val codedOutput = CodedOutputStream(routeBytes) @@ -52,6 +52,11 @@ fun encodeProtoBuf(protoMessage: T): ByteArray { routeBytes = ByteArray(protoSize) val codedOutput = CodedOutputStream(routeBytes) protoMessage.writeTo(codedOutput) + } else if (protoMessage is SonarResponse) { + val protoSize = protoMessage.getSizeNoTag() + routeBytes = ByteArray(protoSize) + val codedOutput = CodedOutputStream(routeBytes) + protoMessage.writeTo(codedOutput) } else { println("PROTO MESSAGE DON'T ENCODE!") routeBytes = ByteArray(0) diff --git a/car_srv/kotlinSrv/src/Main.kt b/car_srv/kotlinSrv/src/Main.kt index 240b2670d16..f2f3fe6ccad 100644 --- a/car_srv/kotlinSrv/src/Main.kt +++ b/car_srv/kotlinSrv/src/Main.kt @@ -4,7 +4,7 @@ import net.server.handlers.AbstractHandler import net.server.handlers.debug.Memory import net.server.handlers.flash.LoadBin import net.server.handlers.main.GetLocation -import net.server.handlers.main.SetRoute +import net.server.handlers.main.GetSonarData import net.server.handlers.rc.Control val carServerPort: Int = 8888 @@ -23,7 +23,7 @@ fun main(args: Array) { val carController = ControllerToUsb() handlers.put("/rc/control", Control()) handlers.put("/loadBin", LoadBin()) - handlers.put("/route", SetRoute(carController)) + handlers.put("/sonar", GetSonarData(carController)) handlers.put("/getLocation", GetLocation()) handlers.put("/debug/memory", Memory()) diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index 1ade6950565..9461ca36e58 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt @@ -3,6 +3,7 @@ package control.emulator import CarState import RouteRequest import RouteResponse +import SonarResponse import control.Controller import encodeProtoBuf import room.Data @@ -46,8 +47,6 @@ class ControllerEmulator : Controller { override fun executeRequestSensorData(angles: IntArray, callBack: (ByteArray) -> Unit) { - //calculate distance - val xSensor0 = CarState.instance.x val ySensor0 = CarState.instance.y val carAngle = CarState.instance.angle @@ -55,10 +54,9 @@ class ControllerEmulator : Controller { val distances = arrayListOf() angles.forEach { angle -> val angleFinal = ((carAngle - angle) % 360) - //tg can be equal to inf if angle = 90. it vertical line and x1 = x0, y1 = y0+[any number. eg 1] - val xSensor1: Int val ySensor1: Double + //tg can be equal to inf if angle = 90. it vertical line. x1 = x0, y1 = y0+[any number. eg 1] if (angleFinal == 90) { xSensor1 = xSensor0 ySensor1 = (ySensor0 + 1).toDouble() @@ -68,6 +66,7 @@ class ControllerEmulator : Controller { } val sensorLine = Line(ySensor0 - ySensor1, xSensor1.toDouble() - xSensor0, xSensor0 * ySensor1 - ySensor0 * xSensor1) + var distance = Int.MAX_VALUE for (wall in Data.walls) { val wallLine = wall.line val coef = sensorLine.A * wallLine.B - sensorLine.B * wallLine.A @@ -80,7 +79,8 @@ class ControllerEmulator : Controller { val xIntersection = (sensorLine.B * wallLine.C - wallLine.B * sensorLine.C) / coef val yIntersection = (sensorLine.C * wallLine.A - wallLine.C * sensorLine.A) / coef - //filter by direction + + //filters by direction and intersection position if (angle > 90 && angle < 270) { //negative direction for OX if (xIntersection >= xSensor0) { @@ -91,13 +91,35 @@ class ControllerEmulator : Controller { continue } } - + if (Math.min(wall.xTo, wall.xFrom) > xIntersection + || Math.max(wall.xTo, wall.xFrom) < xIntersection + || Math.min(wall.yFrom, wall.yTo) > yIntersection + || Math.max(wall.yFrom, wall.yTo) < yIntersection) { + continue + } + val currentDistance = Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0) + + Math.pow(yIntersection - ySensor0, 2.0)).toInt() + if (currentDistance < distance) { + distance = currentDistance + } } - + //Math.random() return double in [0,1) + val delta = getRandomIntFrom(IntArray(5, { x -> x - 2 }))//return one of -2 -1 0 1 or 2 + distances.add(distance + delta) } - + val responseMessage = SonarResponse.BuilderSonarResponse(distances.toIntArray()) + val bytesMessage = encodeProtoBuf(responseMessage) + callBack.invoke(bytesMessage) } + //return random int from array + private fun getRandomIntFrom(values: IntArray): Int { + val randomValue = (Math.random() * 1000).toInt()//value in [0,999] + val randomArrayIdx = randomValue * values.size / 1000//index in [0, values.size-1] + return values[randomArrayIdx] + } + + fun executeCommand(commands: List>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) { if (currentCommandIdx == commands.size) { val responseMessage = RouteResponse.BuilderRouteResponse(0).build() @@ -108,11 +130,13 @@ class ControllerEmulator : Controller { //refresh car state val carInstance = CarState.instance val commandTime = currentCommand.second + val delta = Math.random() * 0.2 + 0.9// delta in [0.9, 1.1) + val commandTimeIncludeRandom = (commandTime * delta).toInt() when (currentCommand.first) { - MoveDirection.FORWARD -> carInstance.moving((commandTime * MOVE_VELOCITY).toInt() / 1000) - MoveDirection.BACKWARD -> carInstance.moving(-(commandTime * MOVE_VELOCITY).toInt() / 1000) - MoveDirection.RIGHT -> carInstance.rotate((commandTime * ROTATION_VELOCITY).toInt() / 1000) - MoveDirection.LEFT -> carInstance.rotate(-(commandTime * ROTATION_VELOCITY).toInt() / 1000) + MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt() / 1000) + MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt() / 1000) + MoveDirection.RIGHT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt() / 1000) + MoveDirection.LEFT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt() / 1000) else -> { } } diff --git a/car_srv/kotlinSrv/src/net/server/handlers/main/GetSonarData.kt b/car_srv/kotlinSrv/src/net/server/handlers/main/GetSonarData.kt new file mode 100644 index 00000000000..be7ab54a247 --- /dev/null +++ b/car_srv/kotlinSrv/src/net/server/handlers/main/GetSonarData.kt @@ -0,0 +1,35 @@ +package net.server.handlers.main + +import CodedInputStream +import McState +import SonarRequest +import SonarResponse +import control.Controller +import encodeProtoBuf +import net.server.handlers.AbstractHandler + +class GetSonarData : AbstractHandler { + + val fromServerObjectBuilder: SonarRequest.BuilderSonarRequest + val toServerObjectBuilder: SonarResponse.BuilderSonarResponse + val controller: Controller + + constructor(routeExecutor: Controller) : super() { + this.controller = routeExecutor + this.fromServerObjectBuilder = SonarRequest.BuilderSonarRequest(IntArray(0)) + this.toServerObjectBuilder = SonarResponse.BuilderSonarResponse(IntArray(0)) + } + + + override fun getBytesResponse(data: ByteArray, callback: (ByteArray) -> Unit) { + val message = fromServerObjectBuilder.build() + message.mergeFrom(CodedInputStream(data)) + if (!McState.instance.isConnected()) { + println("mc is disconnected!") + val responseMessage = toServerObjectBuilder.build() + callback.invoke(encodeProtoBuf(responseMessage)) + return + } + controller.executeRequestSensorData(message.angles, callback) + } +} \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/room/Wall.kt b/car_srv/kotlinSrv/src/room/Wall.kt index b0367d5229d..b8635612274 100644 --- a/car_srv/kotlinSrv/src/room/Wall.kt +++ b/car_srv/kotlinSrv/src/room/Wall.kt @@ -1,5 +1,5 @@ package room -class Wall constructor(val line: Line, val xFrom: Int, val xTo: Int, yFrom: Int, yTo: Int) { +class Wall constructor(val line: Line, val xFrom: Int, val xTo: Int, val yFrom: Int, val yTo: Int) { } \ No newline at end of file