first code for emulator + fixed bugs

car_srv/kotlinSrv/src/CarState.kt

@@ -1,13 +1,24 @@
 class CarState private constructor() {
     //position
-    var x: Double
+    var x: Int
-    var y: Double
+    var y: Int
-    var angle: Double
+    var angle: Int//positive is from OX to OY
 
     init {
-        this.x = 0.0
+        this.x = 0
-        this.y = 0.0
+        this.y = 0
-        this.angle = 0.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 {

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 <T> 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 <T> 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)

car_srv/kotlinSrv/src/MCConnection.kt

@@ -48,13 +48,15 @@ class McTransport() : MCConnectObserver<String> {
             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) {
+                if (messageLength == -1) {
-                    callback.invoke(resultBytes.toByteArray())
-                    resultBytes.clear()
-                } else if (messageLength == -1) {
                     messageLength = getBodyLength(resultBytes)
                 }
+                if (messageLength != -1 && messageLength + protoHeaderLength == resultBytes.size) {
+                    callback.invoke(resultBytes.drop(4).toByteArray())
+                    resultBytes.clear()
+                }
             }
         })
     }

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)
 
 }

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 {
+    override fun executeRequestSensorData(degrees: IntArray, callBack: (ByteArray) -> Unit) {
-        return IntArray(0)//todo make after connect sensor to car
+        //todo make after connect sensor to car
     }
 }

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 MOVE_VELOCITY = 32.78//sm/s
-    private val ROTATION_VELOCITY = 12.3
+    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, callBack)
-        executeCommand(commands, 0)
     }
 
-    override fun getSensorData(degrees: IntArray): IntArray {
+    override fun executeRequestSensorData(degrees: IntArray, callBack: (ByteArray) -> Unit) {
-        throw UnsupportedOperationException("not implemented") //To change body of created functions use File | Settings | File Templates.
+
+//        ByteArray
+
+        //calculate distance
     }
 
-    fun executeCommand(commands: List<Pair<MoveDirection, Int>>, currentCommandIdx: Int) {
+    fun executeCommand(commands: List<Pair<MoveDirection, Int>>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) {
-//        if (currentCommandIdx == commands.size) {
+        if (currentCommandIdx == commands.size) {
-//            MicroController.instance.car.routeDone()
+            val responseMessage = RouteResponse.BuilderRouteResponse(0).build()
-//        }
+            callBack.invoke(encodeProtoBuf(responseMessage))
-//        val currentCommand = commands.get(currentCommandIdx)
+        }
-//        MicroController.instance.car.move(currentCommand.first, currentCommand.second, {
+        val currentCommand = commands.get(currentCommandIdx)
-//            executeCommand(commands, currentCommandIdx + 1)
+
-//        })
+        //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()
-//    }
 
 }

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)
     }
 

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()

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)
+        controller.executeRoute(message, callback)
-        val responseMessage = toServerObjectBuilder.setCode(0).build()
-        callback.invoke(encodeProtoBuf(responseMessage))
     }
 
 }