diff --git a/car_srv/kotlinSrv/src/CarState.kt b/car_srv/kotlinSrv/src/CarState.kt index d189ad1c742..d08bb87a879 100644 --- a/car_srv/kotlinSrv/src/CarState.kt +++ b/car_srv/kotlinSrv/src/CarState.kt @@ -14,7 +14,7 @@ class CarState private constructor() { fun moving(distance: Int) { x += (Math.cos(degreesToRadian(angle)) * distance).toInt() y += (Math.sin(degreesToRadian(angle)) * distance).toInt() - println("new position: {$x, $y}") + println("new position: {$x, $y}, distance: {$distance}") } private fun degreesToRadian(angleInDegrees: Int): Double { @@ -23,7 +23,7 @@ class CarState private constructor() { //angle positive - rotation left fun rotate(angle: Int) { - this.angle += angle + this.angle = (this.angle + angle) % 360 println("new angle: {$angle, ${this.angle}}") } diff --git a/server/src/main/java/RoomScanner/CarController.kt b/server/src/main/java/RoomScanner/CarController.kt index 61b4b474793..8fa99e2a856 100644 --- a/server/src/main/java/RoomScanner/CarController.kt +++ b/server/src/main/java/RoomScanner/CarController.kt @@ -18,15 +18,20 @@ class CarController(var car: Car) { var position = Pair(0.0, 0.0) private set - private val CHARGE_CORRECTION = 1.0 //0.85 + private val CHARGE_CORRECTION = 0.97 + private val MAX_ANGLE = 360.0 + private val SCAN_STEP = 5.0 + private val MIN_ROTATION = 10 + private val MAX_VALID_DISTANCE = 100.0 + private var angle = 0.0 fun moveTo(to: Pair, distance: Double) { - val driveAngle = (Math.toDegrees(Math.atan2(to.first, to.second)) + 360) % 360 + val driveAngle = (Math.toDegrees(Math.atan2(to.second, to.first)) + MAX_ANGLE) % MAX_ANGLE rotateOn(driveAngle) drive(Direction.FORWARD, distance.toInt()) - position = Pair(position.first + distance * Math.cos(Math.toRadians(driveAngle)), + position = Pair(position.first + distance * Math.cos(Math.toRadians(driveAngle)), position.second + distance * Math.sin(Math.toRadians(driveAngle))) } @@ -38,12 +43,38 @@ class CarController(var car: Car) { angle = target } + fun rotateLeftWithCorrection(angle: Double) { + val horizon = horizon() + val before = scan(horizon) + drive(Direction.LEFT, angle.toInt()) + var after = scan(horizon) + + val distance = fun(first: Double, second: Double): Double = Math.max(when { + first == second -> 0.0 + first == -1.0 -> second + second == -1.0 -> first + else -> Math.max(Math.abs(first - second).toDouble(), 100.0) + }, MAX_VALID_DISTANCE) + + var realAngle = (maxSuffix(before, after, distance) + 1) * SCAN_STEP + while (realAngle != angle) { + val direction = if (realAngle > angle) Direction.RIGHT else Direction.LEFT + drive(direction, Math.min(MIN_ROTATION, Math.abs(realAngle - angle).toInt())) + + after = scan(horizon) + realAngle = maxSuffix(before, after, distance) * SCAN_STEP + } + + this.angle = realAngle + } + fun scan(angles: IntArray): List { val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 1, SonarRequest.Smoothing.NONE).build() val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) }) val response = CarClient.sendRequest(car, CarClient.Request.SONAR, data).get().responseBodyAsBytes - return SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances.map { it.toDouble() } + val result = SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances.map { it.toDouble() } + return result } fun convertToPoint(angle: Double, distance: Double): Pair { @@ -55,10 +86,10 @@ class CarController(var car: Car) { return Pair(Math.cos(realAngle) * distance + position.first, Math.sin(realAngle) * distance + position.second) } - private fun drive(direction: Direction, distance: Int) { + fun drive(direction: Direction, distance: Int) { val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf((distance * CHARGE_CORRECTION).toInt()), intArrayOf(direction.id)).build() val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) }) - val response = CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get() + CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get() } } diff --git a/server/src/main/java/RoomScanner/RoomScanner.kt b/server/src/main/java/RoomScanner/RoomScanner.kt index 49d025ed765..2cda005a519 100644 --- a/server/src/main/java/RoomScanner/RoomScanner.kt +++ b/server/src/main/java/RoomScanner/RoomScanner.kt @@ -2,38 +2,39 @@ package RoomScanner class RoomScanner(val controller: CarController) : Thread() { private val points = mutableListOf>() + private val VALID_MEASURE = 150 + private val GHOST_LEVEL = 50 override fun run() { println("Car connected ${controller.car.host}") - while (true) { + for (i in 1..100) { step() println(plot(points)) } + } private fun step() { - val iterationPoints = scan().filter { it.first > 0 || it.second > 0} - points.addAll(iterationPoints) + val iterationPoints = scan().filter { it.first > 0 || it.second > 0 } + points.addAll(iterationPoints.filter { distance(controller.position, it) <= VALID_MEASURE }) - val target = iterationPoints.maxBy { distance(controller.position, it) } - target ?: return - - controller.moveTo(target, 50.0) + val target = iterationPoints.sortedBy { distance(controller.position, it) }.takeLast(GHOST_LEVEL)[(Math.random() * GHOST_LEVEL).toInt()] + controller.moveTo(target, Math.min(distance(controller.position, target), 100.0)) controller.rotateOn(0.0) } private fun scan(): MutableList> { - val horizon = IntArray(180 / 5, { it * 5 }) - horizon.reverse() + val horizon = horizon() val dots = mutableListOf>() for (i in arrayOf(0.0, 90.0, 180.0, 270.0)) { - controller.rotateOn(i) + controller.rotateLeftWithCorrection(90.0) dots.addAll(controller.scan(horizon).mapIndexed { i: Int, d: Double -> controller.convertToPoint((i * 5 - 180).toDouble(), d) }) + controller.drive(CarController.Direction.FORWARD, 10) + controller.drive(CarController.Direction.BACKWARD, 10) } - controller.rotateOn(0.0) - + controller.rotateLeftWithCorrection(90.0) return dots } diff --git a/server/src/main/java/RoomScanner/utils.kt b/server/src/main/java/RoomScanner/utils.kt index d8756df94f1..8cc6d87c780 100644 --- a/server/src/main/java/RoomScanner/utils.kt +++ b/server/src/main/java/RoomScanner/utils.kt @@ -1,5 +1,12 @@ package RoomScanner +fun horizon(): IntArray { + val horizon = IntArray(180 / 5, { it * 5 }) + horizon.reverse() + + return horizon +} + fun distance(first: Pair, second: Pair): Double { val xDistance = first.first - second.first val yDistance = first.second - second.second @@ -7,9 +14,6 @@ fun distance(first: Pair, second: Pair): Double return Math.sqrt(xDistance * xDistance + yDistance * yDistance) } -fun estimateAngle(from: Pair, to: Pair): Double = - Math.atan2(to.first - from.first, to.second - from.second) - fun angleDistance(from: Double, to: Double): Double { val distance = Math.min(Math.abs(from - to), Math.abs(360 - (from - to))) val direction = if (distance == Math.abs(from - to)) Math.signum(to - from) else -Math.signum(to - from)