From 10acb69eb3baeeb92591f5fb5a75b00f796e4bd7 Mon Sep 17 00:00:00 2001
From: MaximZaitsev <AnterKan1@gmail.com>
Date: Wed, 24 Aug 2016 13:52:38 +0300
Subject: [PATCH] add test, remove time delay in car move, fixed bug with sonar
 emulator on angle between 90 and 270

---
 car_srv/kotlinSrv/src/Test.kt                         | 11 +++++++++++
 .../src/control/emulator/ControllerEmulator.kt        |  8 ++++++--
 2 files changed, 17 insertions(+), 2 deletions(-)

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