diff --git a/car_srv/kotlinSrv/src/Test.kt b/car_srv/kotlinSrv/src/Test.kt index 57b8e213280..2ba16f1275b 100644 --- a/car_srv/kotlinSrv/src/Test.kt +++ b/car_srv/kotlinSrv/src/Test.kt @@ -17,6 +17,17 @@ private fun testCarEmulator() { eq(dist90.toDouble(), Room.bottomLine.C / Room.bottomLine.B, "check angle 90", 2.5) }) + println("--------------------") + sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(2, { 120 + 60 * it })).build() + sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build() + controller.executeRequestSensorData(sonarRequest, { bytes -> + sonarResponse.mergeFrom(CodedInputStream(bytes)) + val dist120 = sonarResponse.distances[0] + val dist180 = sonarResponse.distances[1] + eq(dist120.toDouble(), 23.094, "check angle 120", 2.5) + eq(dist180.toDouble(), 150.0, "check angle 180", 2.5) + }) + println("--------------------") CarState.instance.x = 50 CarState.instance.y = 50 diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index 1cc0c93c1b3..fb3f461926b 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt @@ -69,6 +69,10 @@ class ControllerEmulator : Controller { xSensor1 = xSensor0 ySensor1 = (ySensor0 - 1).toDouble() } + in (90..270) -> { + xSensor1 = xSensor0 - 1 + ySensor1 = ySensor0 + (xSensor1 - xSensor0) * Math.tan(angleFinal * Math.PI / 180) + } else -> { xSensor1 = xSensor0 + 1 ySensor1 = ySensor0 + (xSensor1 - xSensor0) * Math.tan(angleFinal * Math.PI / 180) @@ -164,9 +168,9 @@ class ControllerEmulator : Controller { } } - setTimeout({ +// setTimeout({ executeCommand(commands, currentCommandIdx + 1, callBack) - }, currentCommand.second) +// }, currentCommand.second) }