diff --git a/car_fmw/src/Control.kt b/car_fmw/src/Control.kt index 584240a61b5..9c2e5dc7f02 100644 --- a/car_fmw/src/Control.kt +++ b/car_fmw/src/Control.kt @@ -87,7 +87,8 @@ object Control { val data = IntArray(attempts) var i = 0 while (i < attempts) { - data[i] = Sonar.getSmoothDistance(angle, windowSize) + data[i] = Sonar.getSmoothDistance(angle, windowSize, smoothing) + if (smoothing.id == SonarRequest.Smoothing.NONE.id && data[i] != -1) { return data[i] } @@ -95,8 +96,8 @@ object Control { } return when (smoothing.id) { - SonarRequest.Smoothing.MEAN.id -> data.mean() - SonarRequest.Smoothing.MEDIAN.id -> data.median() + SonarRequest.Smoothing.MEAN.id -> data.filter(::positive).mean() + SonarRequest.Smoothing.MEDIAN.id -> data.filter(::positive).median() else -> -1 } } diff --git a/car_fmw/src/Voyager.kt b/car_fmw/src/Voyager.kt index eff15a338f9..9951151e2ba 100644 --- a/car_fmw/src/Voyager.kt +++ b/car_fmw/src/Voyager.kt @@ -6,10 +6,10 @@ object Voyager { fun run() { while (true) { - var distance = Sonar.getSmoothDistance(0, DEFAULT_WINDOW) + var distance = Sonar.getSmoothDistance(0, DEFAULT_WINDOW, SonarRequest.Smoothing.NONE) while (distance == -1 || distance > 4 * SEGMENT_SIZE) { Engine.drive(RouteType.FORWARD.id, SEGMENT_SIZE) - distance = Sonar.getSmoothDistance(0, DEFAULT_WINDOW) + distance = Sonar.getSmoothDistance(0, DEFAULT_WINDOW, SonarRequest.Smoothing.NONE) } Engine.drive(RouteType.LEFT.id, 45) diff --git a/car_fmw/src/include/Sonar.kt b/car_fmw/src/include/Sonar.kt index e729d7f9156..221b69a605b 100644 --- a/car_fmw/src/include/Sonar.kt +++ b/car_fmw/src/include/Sonar.kt @@ -2,8 +2,6 @@ external fun car_sonar_init() external fun car_sonar_get_dist(degree: Byte): Short object Sonar { - private var SCAN_STEP = 1 - fun init() { car_sonar_init() } @@ -11,32 +9,67 @@ object Sonar { fun getDistance(angle: Int): Int = car_sonar_get_dist(angle.toByte()).toInt() - fun getSmoothDistance(angle: Int, windowSize: Int): Int { + fun getSmoothDistance(angle: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): Int { val distance = getDistance(angle) if (distance != -1 || windowSize == 0) { return distance } val start = if (angle - windowSize < 0) 0 else angle - windowSize - return getOneOfRange(start, angle + windowSize, SCAN_STEP) + val stop = angle + windowSize + when (smoothing.id) { + SonarRequest.Smoothing.NONE.id -> { + return getOneOfRange(start, stop) + } + SonarRequest.Smoothing.MEDIAN.id -> { + val data = getRange(start, stop).filter(::positive) + if (data.size == 0) { + return -1 + } + + return data.median() + } + SonarRequest.Smoothing.MEAN.id -> { + val data = getRange(start, stop).filter(::positive) + if (data.size == 0) { + return -1 + } + + return data.mean() + } + } + + return -1 } - fun getOneOfRange(start: Int, stop: Int, step: Int): Int { + private fun getOneOfRange(start: Int, stop: Int): Int { var i = start var distance = getDistance(start) while (i <= stop && distance == -1) { distance = getDistance(i) - i += step + i += 1 } while (i >= start && distance == -1) { distance = getDistance(i) - i -= step + i -= 1 } return distance } + private fun getRange(start: Int, stop: Int): IntArray { + val result = IntArray(stop - start + 1) + var i = 0 + while (i < stop - start + 1) { + result[i] = getDistance(start + i) + i++ + } + + return result + } + + fun calibrate() { getDistance(180) } diff --git a/car_fmw/src/include/utils.kt b/car_fmw/src/include/utils.kt new file mode 100644 index 00000000000..75a1bcf4409 --- /dev/null +++ b/car_fmw/src/include/utils.kt @@ -0,0 +1,4 @@ + +fun positive(i: Int): Boolean { + return i > 0 +} diff --git a/server/src/main/java/RoomScanner/CarController.kt b/server/src/main/java/RoomScanner/CarController.kt index a9528065b85..f4b97da6a47 100644 --- a/server/src/main/java/RoomScanner/CarController.kt +++ b/server/src/main/java/RoomScanner/CarController.kt @@ -16,15 +16,14 @@ class CarController(var car: Car) { RIGHT(3); } - val WALL_DISTANCE = 50 - var position = Pair(0.0, 0.0) private set + private val WALL_DISTANCE = 50 private var angle = 0.0 fun moveTo(to: Pair) { - var driveAngle = (Math.toDegrees(Math.atan2(to.first, to.second)) + 360) % 360 + val driveAngle = (Math.toDegrees(Math.atan2(to.first, to.second)) + 360) % 360 rotateOn(angleDistance(angle, driveAngle)) drive(Direction.FORWARD, Math.max(0.0, distance(position, to)).toInt() - WALL_DISTANCE) @@ -39,12 +38,6 @@ class CarController(var car: Car) { angle = target } - private fun drive(direction: Direction, distance: Int) { - val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf(distance), intArrayOf(direction.id)).build() - val data = serialize(request.getSizeNoTag(), { i -> request.writeTo(i) }) - CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get() - } - fun scan(angles: IntArray): List> { val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 10, SonarRequest.Smoothing.MEDIAN).build() val data = serialize(request.getSizeNoTag(), { i -> request.writeTo(i) }) @@ -55,7 +48,13 @@ class CarController(var car: Car) { return distances.mapIndexed { i: Int, distance: Int -> convertToPoint(angles[i].toDouble(), distance.toDouble()) } } - fun convertToPoint(angle: Double, distance: Double): Pair { + private fun drive(direction: Direction, distance: Int) { + val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf(distance), intArrayOf(direction.id)).build() + val data = serialize(request.getSizeNoTag(), { i -> request.writeTo(i) }) + CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get() + } + + private fun convertToPoint(angle: Double, distance: Double): Pair { if (distance <= 0) { return Pair(0.0, 0.0) } @@ -64,7 +63,7 @@ class CarController(var car: Car) { return Pair(Math.cos(realAngle) * distance + position.first, Math.sin(realAngle) * distance + position.second) } - inline fun serialize(size: Int, writeTo: (CodedOutputStream) -> Unit): ByteArray { + private inline fun serialize(size: Int, writeTo: (CodedOutputStream) -> Unit): ByteArray { val bytes = ByteArray(size) writeTo(CodedOutputStream(bytes)) diff --git a/server/src/main/java/RoomScanner/RoomScanner.kt b/server/src/main/java/RoomScanner/RoomScanner.kt index 7d958564fab..4dbdf5f4144 100644 --- a/server/src/main/java/RoomScanner/RoomScanner.kt +++ b/server/src/main/java/RoomScanner/RoomScanner.kt @@ -2,6 +2,7 @@ package RoomScanner class RoomScanner(val controller: CarController) : Thread() { private val points = mutableListOf>() + private val MAX_VALID_DISTANCE = 100 override fun run() { println("Car connected ${controller.car.host}") @@ -20,7 +21,7 @@ class RoomScanner(val controller: CarController) : Thread() { controller.rotateOn(180.0) iterationPoints.addAll(controller.scan(horizon)) - points.addAll(iterationPoints) + points.addAll(iterationPoints.filter { it.first > 0 && it.second > 0 && distance(controller.position, it) <= MAX_VALID_DISTANCE }) val target = iterationPoints.filter { it.first > 0 && it.second > 0 }.maxBy { distance(controller.position, it) } target ?: return