diff --git a/car_srv/kotlinSrv/src/Main.kt b/car_srv/kotlinSrv/src/Main.kt index 2f915fe4127..303fe4016d7 100644 --- a/car_srv/kotlinSrv/src/Main.kt +++ b/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) { val handlers: MutableMap = 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()) diff --git a/car_srv/kotlinSrv/src/Test.kt b/car_srv/kotlinSrv/src/Test.kt new file mode 100644 index 00000000000..b17367712d6 --- /dev/null +++ b/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") + } +} diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index b47fcc0e23a..b46bde4b79f 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/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() 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] - if (angleFinal == 90) { - xSensor1 = xSensor0 - ySensor1 = (ySensor0 + 1).toDouble() - } else { - xSensor1 = xSensor0 + 1 - ySensor1 = xSensor1 * Math.tan(angleFinal.toDouble()) + //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() + } + 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) { - //negative direction for OX - if (xIntersection >= xSensor0) { - 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) { + val IntersectionVectorX = xIntersection - xSensor0 + val IntersectionVectorY = yIntersection - ySensor0 + if (IntersectionVectorX * sensorVectorX + IntersectionVectorY * sensorVectorY < 0) { continue } - val currentDistance = Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0) - + Math.pow(yIntersection - ySensor0, 2.0)).toInt() + val wallVectorXTo = wall.xTo - xIntersection + 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()) - //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) + println("dist $distance") + if (distance < 5 || distance > 500) { + distances.add(-1) + } 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)