azalea/kotlin-fork
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

car_srv: horizon merge, fix rotation, minor fixes

Browse Source
This commit is contained in:
e5l
2016-08-31 18:09:47 +03:00
parent baddaa0755
commit 5c3c63a60a
5 changed files with 79 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
car_hw/src/car_sonar.c
+1 -1
View File
@@ -49,7 +49,7 @@ static uint16_t sonar_wait_dist_resp(void)
size_t i = 0;
uint8_t sum = 0;
uint32_t distance = 0;
int16_t distance = 0;
usart_rcv_data(USART3_ID, sonar_resp, 4);
sum = sonar_resp[0] + sonar_resp[1] + sonar_resp[2];
server/src/main/java/RoomScanner/CarController.kt
+11 -13
View File
@@ -19,6 +19,7 @@ class CarController(var car: Car) {
private set
private val WALL_DISTANCE = 50
private val CHARGE_CORRECTION = 1.0 //0.85
private var angle = 0.0
fun moveTo(to: Pair<Double, Double>) {
@@ -31,29 +32,21 @@ class CarController(var car: Car) {
fun rotateOn(target: Double) {
val rotateAngle = angleDistance(angle, target)
val direction = if (rotateAngle > 0) Direction.RIGHT else Direction.LEFT
val direction = if (rotateAngle > 0) Direction.LEFT else Direction.RIGHT
drive(direction, rotateAngle.toInt())
angle = target
}
fun scan(angles: IntArray): List<Pair<Double, Double>> {
val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN).build()
fun scan(angles: IntArray): List<Double> {
val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 5, 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
val distances = SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances
return distances.mapIndexed { i: Int, distance: Int -> convertToPoint(angles[i].toDouble(), distance.toDouble()) }
return SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances.map { it.toDouble() }
}
private fun drive(direction: Direction, distance: Int) {
val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf(distance), intArrayOf(direction.id)).build()
val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get()
}
private fun convertToPoint(angle: Double, distance: Double): Pair<Double, Double> {
fun convertToPoint(angle: Double, distance: Double): Pair<Double, Double> {
if (distance <= 0) {
return Pair(0.0, 0.0)
}
@@ -62,5 +55,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) {
val request = RouteMetricRequest.BuilderRouteMetricRequest(intArrayOf((distance * CHARGE_CORRECTION).toInt()), intArrayOf(direction.id)).build()
val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) })
CarClient.sendRequest(car, CarClient.Request.ROUTE_METRIC, data).get()
}
}
server/src/main/java/RoomScanner/RoomScanner.kt
+32 -12
View File
@@ -6,22 +6,20 @@ class RoomScanner(val controller: CarController) : Thread() {
override fun run() {
println("Car connected ${controller.car.host}")
var i = 1
while (true) {
scan()
println(plot(points))
step()
if (i % 10 == 0) {
println(plot(points))
}
i++
}
}
private fun scan() {
val horizon = IntArray(180 / 5, { it * 5 })
val iterationPoints = mutableListOf<Pair<Double, Double>>()
controller.rotateOn(0.0)
iterationPoints.addAll(controller.scan(horizon))
controller.rotateOn(180.0)
iterationPoints.addAll(controller.scan(horizon))
points.addAll(iterationPoints.filter { it.first > 0 && it.second > 0 && distance(controller.position, it) <= MAX_VALID_DISTANCE })
private fun step() {
val iterationPoints = scan()
points.addAll(iterationPoints.filter { it.first > 0 && it.second > 0 })
val target = iterationPoints.filter { it.first > 0 && it.second > 0 }.maxBy { distance(controller.position, it) }
target ?: return
@@ -29,6 +27,28 @@ class RoomScanner(val controller: CarController) : Thread() {
controller.moveTo(target)
}
private fun scan(): MutableList<Pair<Double, Double>> {
val horizon = IntArray(180 / 5, { it * 5 })
val distance = fun(first: Double, second: Double): Double = when {
first == second -> 0.0
first == -1.0 -> second
second == -1.0 -> first
else -> Math.abs(first - second)
}
val metrics = mutableListOf<List<Double>>()
for (i in intArrayOf(0, 90, 180, 270)) {
controller.rotateOn(i.toDouble())
metrics.add(controller.scan(horizon))
}
val points = merge(metrics.reversed(), distance).reversed()
controller.rotateOn(0.0)
return points.mapIndexed { i: Int, d: Double -> controller.convertToPoint((i * 5).toDouble(), d) }.toMutableList()
}
private fun plot(points: MutableList<Pair<Double, Double>>) =
"x <- c(${points.joinToString { it.first.toInt().toString() }}) \n y <- c(${points.joinToString { it.second.toInt().toString() }})"
}
server/src/main/java/RoomScanner/utils.kt
+35 -1
View File
@@ -18,4 +18,38 @@ fun angleDistance(from: Double, to: Double): Double {
val down = 360 - max + min
if (Math.abs(up) < Math.abs(down)) return up else return down
}
}
fun <T> maxSuffix(first: List<T>, second: List<T>, distance: (T, T) -> Double): Int {
val distances = first.mapIndexed { i: Int, t: T ->
var score = 0.0
var size = Math.min(second.size - 1, first.size - i - 1)
for (j in 0..size) {
score += distance(first[i + j], second[j])
}
score / (size + 1)
}.toList()
return distances.indexOf(distances.min())
}
fun <T> merge(lists: List<List<T>>, distance: (T, T) -> Double): List<T> {
val indexes = mutableListOf<Int>()
for (i in 0..(lists.size - 2)) {
indexes.add(maxSuffix(lists[i], lists[i + 1], distance))
}
val result = mutableListOf<T>()
for (i in 0..(indexes.size - 1)) {
for (j in 0..indexes[i]) {
result.add(lists[i][j])
}
}
for (i in (indexes.last() + 1)..(lists.last().size - 1)) {
result.add(lists.last()[i])
}
return result
}
server/src/main/java/utils.kt
View File