diff --git a/car_srv/kotlinSrv/src/CarState.kt b/car_srv/kotlinSrv/src/CarState.kt index ce264c5ac09..759891ebf5a 100644 --- a/car_srv/kotlinSrv/src/CarState.kt +++ b/car_srv/kotlinSrv/src/CarState.kt @@ -1,13 +1,24 @@ class CarState private constructor() { //position - var x: Double - var y: Double - var angle: Double + var x: Int + var y: Int + var angle: Int//positive is from OX to OY init { - this.x = 0.0 - this.y = 0.0 - this.angle = 0.0 + this.x = 0 + this.y = 0 + this.angle = 0 + } + + //if distance is positive - move forward, else backward + fun moving(distance: Int) { + x += (Math.cos(angle.toDouble()) * distance).toInt() + y += (Math.sin(angle.toDouble()) * distance).toInt() + } + + //angle positive - rotation left + fun rotate(angle: Int) { + this.angle += angle } companion object { diff --git a/car_srv/kotlinSrv/src/JSImport.kt b/car_srv/kotlinSrv/src/JSImport.kt index 3df3171e8be..f748c7d4093 100644 --- a/car_srv/kotlinSrv/src/JSImport.kt +++ b/car_srv/kotlinSrv/src/JSImport.kt @@ -1,8 +1,12 @@ @native fun require(name: String): dynamic = noImpl +@native +fun setTimeout(callBack: () -> Unit, ms: Int): dynamic = noImpl + fun encodeProtoBuf(protoMessage: T): ByteArray { val routeBytes: ByteArray + println(protoMessage.toString()) if (protoMessage is LocationResponse) { val protoSize = protoMessage.getSizeNoTag() routeBytes = ByteArray(protoSize) @@ -18,6 +22,11 @@ fun encodeProtoBuf(protoMessage: T): ByteArray { routeBytes = ByteArray(protoSize) val codedOutput = CodedOutputStream(routeBytes) protoMessage.writeTo(codedOutput) + }else if (protoMessage is DebugRequest) { + val protoSize = protoMessage.getSizeNoTag() + routeBytes = ByteArray(protoSize) + val codedOutput = CodedOutputStream(routeBytes) + protoMessage.writeTo(codedOutput) } else if (protoMessage is DirectionResponse) { val protoSize = protoMessage.getSizeNoTag() routeBytes = ByteArray(protoSize) diff --git a/car_srv/kotlinSrv/src/MCConnection.kt b/car_srv/kotlinSrv/src/MCConnection.kt index 9323879c26a..3534f13b21b 100644 --- a/car_srv/kotlinSrv/src/MCConnection.kt +++ b/car_srv/kotlinSrv/src/MCConnection.kt @@ -48,13 +48,15 @@ class McTransport() : MCConnectObserver { var messageLength = getBodyLength(resultBytes) for (i in 0..data.length - 1) { + println("read byte :" + data[i]) resultBytes.add(data[i]) - if (messageLength != -1 && messageLength + protoHeaderLength == resultBytes.size) { - callback.invoke(resultBytes.toByteArray()) - resultBytes.clear() - } else if (messageLength == -1) { + if (messageLength == -1) { messageLength = getBodyLength(resultBytes) } + if (messageLength != -1 && messageLength + protoHeaderLength == resultBytes.size) { + callback.invoke(resultBytes.drop(4).toByteArray()) + resultBytes.clear() + } } }) } diff --git a/car_srv/kotlinSrv/src/control/Controller.kt b/car_srv/kotlinSrv/src/control/Controller.kt index 3e48056b1da..37fb05912d0 100644 --- a/car_srv/kotlinSrv/src/control/Controller.kt +++ b/car_srv/kotlinSrv/src/control/Controller.kt @@ -4,8 +4,8 @@ import RouteRequest interface Controller { - fun executeRoute(route: RouteRequest) + fun executeRoute(route: RouteRequest, callBack: (ByteArray) -> Unit) - fun getSensorData(degrees: IntArray): IntArray + fun executeRequestSensorData(degrees: IntArray, callBack: (ByteArray) -> Unit) } \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt b/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt index b2f0f9c988b..ccc8aefa5c4 100644 --- a/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt +++ b/car_srv/kotlinSrv/src/control/car/ControllerToUsb.kt @@ -6,17 +6,17 @@ import mcTransport class ControllerToUsb : Controller { - override fun executeRoute(route: RouteRequest) { + override fun executeRoute(route: RouteRequest, callBack: (ByteArray) -> Unit) { println("Execute Route:") mcTransport.setCallBack { bytes -> - println("Read $bytes;") + callBack.invoke(bytes) } mcTransport.sendProtoBuf(route) } - override fun getSensorData(degrees: IntArray): IntArray { - return IntArray(0)//todo make after connect sensor to car + override fun executeRequestSensorData(degrees: IntArray, callBack: (ByteArray) -> Unit) { + //todo make after connect sensor to car } } \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index 33169653f8c..ee3292d7797 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt @@ -1,12 +1,17 @@ package control.emulator +import CarState import RouteRequest +import RouteResponse import control.Controller +import encodeProtoBuf +import setTimeout +import kotlin.Pair class ControllerEmulator : Controller { - private val MOVE_VELOCITY = 0.3278 - private val ROTATION_VELOCITY = 12.3 + private val MOVE_VELOCITY = 32.78//sm/s + private val ROTATION_VELOCITY = 12.3//degrees/s enum class MoveDirection { LEFT, @@ -16,7 +21,7 @@ class ControllerEmulator : Controller { ERROR } - override fun executeRoute(route: RouteRequest) { + 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 @@ -34,75 +39,40 @@ class ControllerEmulator : Controller { commands.add(Pair(moveDirection, value)) } - - executeCommand(commands, 0) + executeCommand(commands, 0, callBack) } - override fun getSensorData(degrees: IntArray): IntArray { - throw UnsupportedOperationException("not implemented") //To change body of created functions use File | Settings | File Templates. + override fun executeRequestSensorData(degrees: IntArray, callBack: (ByteArray) -> Unit) { + +// ByteArray + + //calculate distance } - fun executeCommand(commands: List>, currentCommandIdx: Int) { -// if (currentCommandIdx == commands.size) { -// MicroController.instance.car.routeDone() -// } -// val currentCommand = commands.get(currentCommandIdx) -// MicroController.instance.car.move(currentCommand.first, currentCommand.second, { -// executeCommand(commands, currentCommandIdx + 1) -// }) + fun executeCommand(commands: List>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) { + if (currentCommandIdx == commands.size) { + val responseMessage = RouteResponse.BuilderRouteResponse(0).build() + callBack.invoke(encodeProtoBuf(responseMessage)) + } + val currentCommand = commands.get(currentCommandIdx) + + //refresh car state + val carInstance = CarState.instance + val commandTime = currentCommand.second + 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) + else -> { + } + } + + setTimeout({ + executeCommand(commands, currentCommandIdx + 1, callBack) + }, currentCommand.second) } -// var moveDirection: MoveDirection = MoveDirection.STOP -// -// fun stopCar() { -// move(MoveDirection.STOP, 0, {}) -// } -// -// fun refreshLocation(delta: Int) { -// val deltaSeconds = delta.toDouble() / 1000 -// when (moveDirection) { -// MoveDirection.FORWARD -> { -// this.x += MOVE_VELOCITY * deltaSeconds * Math.cos(this.angle * Math.PI / 180); -// this.y += MOVE_VELOCITY * deltaSeconds * Math.sin(this.angle * Math.PI / 180); -// } -// MoveDirection.BACKWARD -> { -// this.x -= MOVE_VELOCITY * deltaSeconds * Math.cos(this.angle * Math.PI / 180); -// this.y -= MOVE_VELOCITY * deltaSeconds * Math.sin(this.angle * Math.PI / 180); -// } -// MoveDirection.LEFT -> this.angle += ROTATION_VELOCITY * deltaSeconds -// MoveDirection.RIGHT -> this.angle -= ROTATION_VELOCITY * deltaSeconds -// else -> { -// -// } -// } -//// println("x=$x; y=$y; angle=$angle") -// } -// -// fun routeDone() { -// controller.stopCar() -// } -// -// fun move(moveDirection: MoveDirection, value: Int, callBack: () -> Unit) { -// //value - angle for rotation command and distance for forward/backward command -// this.moveDirection = moveDirection -// when (moveDirection) { -// MoveDirection.STOP -> controller.stopCar() -// MoveDirection.FORWARD -> controller.moveCarForward() -// MoveDirection.BACKWARD -> controller.moveCarBackward() -// MoveDirection.LEFT -> controller.moveCarLeft() -// MoveDirection.RIGHT -> controller.moveCarRight() -// } -// if (moveDirection != MoveDirection.STOP) { -// if (moveDirection == MoveDirection.FORWARD || moveDirection == MoveDirection.BACKWARD) { -// controller.delay(getTimeForMoving(value, MOVE_VELOCITY), callBack) -// } else { -// controller.delay(getTimeForMoving(value, ROTATION_VELOCITY), callBack) -// } -// } -// } -// -// fun getTimeForMoving(value: Int, velocity: Double): Int { -// return (1000 * Math.abs(value.toDouble()) / velocity).toInt() -// } + } \ No newline at end of file diff --git a/car_srv/kotlinSrv/src/net/server/handlers/debug/Memory.kt b/car_srv/kotlinSrv/src/net/server/handlers/debug/Memory.kt index 36966aeaee7..35864dfbfce 100644 --- a/car_srv/kotlinSrv/src/net/server/handlers/debug/Memory.kt +++ b/car_srv/kotlinSrv/src/net/server/handlers/debug/Memory.kt @@ -1,12 +1,12 @@ package net.server.handlers.debug +import CodedInputStream import DebugRequest import DebugResponseMemoryStats import McState import encodeProtoBuf import mcTransport import net.server.handlers.AbstractHandler -import CodedInputStream class Memory : AbstractHandler { @@ -14,7 +14,7 @@ class Memory : AbstractHandler { val toServerObjectBuilder: DebugResponseMemoryStats.BuilderDebugResponseMemoryStats constructor() : super() { - fromServerObjectBuilder = DebugRequest.BuilderDebugRequest(DebugRequest.TYPE.MEMORYSTATS) + fromServerObjectBuilder = DebugRequest.BuilderDebugRequest(DebugRequest.Type.MEMORY_STATS) toServerObjectBuilder = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(0, 0, 0, 0) } diff --git a/car_srv/kotlinSrv/src/net/server/handlers/flash/LoadBin.kt b/car_srv/kotlinSrv/src/net/server/handlers/flash/LoadBin.kt index f08d697ec9b..023f66d7c48 100644 --- a/car_srv/kotlinSrv/src/net/server/handlers/flash/LoadBin.kt +++ b/car_srv/kotlinSrv/src/net/server/handlers/flash/LoadBin.kt @@ -24,7 +24,7 @@ class LoadBin : AbstractHandler { override fun getBytesResponse(data: ByteArray, callback: (b: ByteArray) -> Unit) { val message = fromServerObjectBuilder.build() message.mergeFrom(CodedInputStream(data)) - fs.writeFile("./flash.bin", js("new Buffer(data)"), fun(err: dynamic) { + fs.writeFile("./flash.bin", js("new Buffer(data)"), { err, stdOut, stdErr -> if (err) { println("error in save flash.bin file\n $err") val responseMessage = toServerObjectBuilder.setResultCode(14).build() diff --git a/car_srv/kotlinSrv/src/net/server/handlers/main/SetRoute.kt b/car_srv/kotlinSrv/src/net/server/handlers/main/SetRoute.kt index 5e5a2ea05e0..c3b80a3f7b2 100644 --- a/car_srv/kotlinSrv/src/net/server/handlers/main/SetRoute.kt +++ b/car_srv/kotlinSrv/src/net/server/handlers/main/SetRoute.kt @@ -29,9 +29,7 @@ class SetRoute : AbstractHandler { callback.invoke(encodeProtoBuf(responseMessage)) return } - controller.executeRoute(message) - val responseMessage = toServerObjectBuilder.setCode(0).build() - callback.invoke(encodeProtoBuf(responseMessage)) + controller.executeRoute(message, callback) } } \ No newline at end of file