fixed bugs in emulator

car_srv/kotlinSrv/src/Main.kt

@@ -6,6 +6,7 @@ import net.server.handlers.debug.Memory
 import net.server.handlers.flash.LoadBin
 import net.server.handlers.main.GetLocation
 import net.server.handlers.main.GetSonarData
+import net.server.handlers.main.SetRoute
 import net.server.handlers.rc.Control
 
 val carServerPort: Int = 8888
@@ -21,11 +22,12 @@ fun main(args: Array<String>) {
 
     val handlers: MutableMap<String, AbstractHandler> = mutableMapOf()
 
-    val carController = ControllerToUsb()
+    val carController = ControllerEmulator()
 
     handlers.put("/rc/control", Control())
     handlers.put("/loadBin", LoadBin())
     handlers.put("/sonar", GetSonarData(carController))
+    handlers.put("/route", SetRoute(carController))
     handlers.put("/getLocation", GetLocation())
     handlers.put("/debug/memory", Memory())
 

car_srv/kotlinSrv/src/Test.kt

@@ -0,0 +1,59 @@
+import control.emulator.ControllerEmulator
+import room.Room
+
+/**
+ * Created by user on 8/22/16.
+ */
+fun runTests() {
+    testCarEmulator()
+}
+
+private fun testCarEmulator() {
+    val controller = ControllerEmulator()
+    var sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(2, { x -> x * 90 })).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(), Math.abs(Room.rightLine.C / Room.rightLine.A), "check angle 0", 2.5)
+        eq(dist90.toDouble(), Room.bottomLine.C / Room.bottomLine.B, "check angle 90", 2.5)
+    })
+
+    println("--------------------")
+    CarState.instance.x = 50
+    CarState.instance.y = 50
+    CarState.instance.angle = 90
+    sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(3, { x -> x * 90 })).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 })).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)
+    })
+}
+
+
+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")
+    }
+}

car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt

@@ -6,7 +6,7 @@ import RouteResponse
 import SonarResponse
 import control.Controller
 import encodeProtoBuf
-import room.Data
+import room.Room
 import room.Line
 import setTimeout
 import kotlin.Pair
@@ -55,60 +55,69 @@ class ControllerEmulator : Controller {
 
         val distances = arrayListOf<Int>()
         angles.forEach { angle ->
-            val angleFinal = ((carAngle - angle) % 360)
+            //need angle in [0,360)
+            var angleTmp = carAngle - angle
+            while (angleTmp < 0) {
+                angleTmp += 360
+            }
+            val angleFinal = angleTmp % 360
             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]
+            //tg can be equal to inf if angle = 90 or 270. it vertical line. x1 = x0, y1 = y0+-[any number. eg 1]
-            if (angleFinal == 90) {
+            when (angleFinal) {
-                xSensor1 = xSensor0
+                90 -> {
-                ySensor1 = (ySensor0 + 1).toDouble()
+                    xSensor1 = xSensor0
-            } else {
+                    ySensor1 = (ySensor0 + 1).toDouble()
-                xSensor1 = xSensor0 + 1
+                }
-                ySensor1 = xSensor1 * Math.tan(angleFinal.toDouble())
+                270 -> {
+                    xSensor1 = xSensor0
+                    ySensor1 = (ySensor0 - 1).toDouble()
+                }
+                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 sensorVectorX = xSensor1 - xSensor0
+            val sensorVectorY = ySensor1 - ySensor0
             var distance = Int.MAX_VALUE
-            for (wall in Data.walls) {
+            for (wall in Room.walls) {
                 val wallLine = wall.line
                 val coef = sensorLine.A * wallLine.B - sensorLine.B * wallLine.A
                 if (Math.abs(coef) < 0.01) {
                     //line is parallel.
                     continue
                 }
-                //sensor line is A1
-                //x=(B1*C2 - B2*C1)/coef   y = (C1A2-C2A1)/coef
                 val xIntersection = (sensorLine.B * wallLine.C - wallLine.B * sensorLine.C) / coef
                 val yIntersection = (sensorLine.C * wallLine.A - wallLine.C * sensorLine.A) / coef
 
-
                 //filters by direction and intersection position
-                if (angle > 90 && angle < 270) {
+                val IntersectionVectorX = xIntersection - xSensor0
-                    //negative direction for OX
+                val IntersectionVectorY = yIntersection - ySensor0
-                    if (xIntersection >= xSensor0) {
+                if (IntersectionVectorX * sensorVectorX + IntersectionVectorY * sensorVectorY < 0) {
-                        continue
-                    }
-                } else {
-                    if (xIntersection < xSensor0) {
-                        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)
+                val wallVectorXTo = wall.xTo - xIntersection
-                        + Math.pow(yIntersection - ySensor0, 2.0)).toInt()
+                val wallVectorYTo = wall.yTo - yIntersection
+                val wallVectorXFrom = wall.xFrom - xIntersection
+                val wallVectorYFrom = wall.yFrom - yIntersection
+                if (wallVectorXTo * wallVectorXFrom + wallVectorYTo * wallVectorYFrom > 0) {
+                    continue
+                }
+                val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0)
+                        + Math.pow(yIntersection - ySensor0, 2.0)))
                 if (currentDistance < distance) {
                     distance = currentDistance
                 }
             }
-            println("dist " + distance.toString())
+            println("dist $distance")
-            //Math.random() return double in [0,1)
+            if (distance < 5 || distance > 500) {
-            val delta = getRandomIntFrom(IntArray(5, { x -> x - 2 }))//return one of -2 -1 0 1 or 2
+                distances.add(-1)
-            distances.add(distance + delta)
+            } else {
+                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()).build()
         val bytesMessage = encodeProtoBuf(responseMessage)