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

algorithm refactoring

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-29 12:08:39 +03:00
parent 9fba3d1b6e
commit dc16f3e480
8 changed files with 116 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
server/src/main/java/DebugClInterface.kt
+1 -2
View File
@@ -1,4 +1,3 @@
import Exceptions.InactiveCarException import Exceptions.InactiveCarException
import algorithm.AbstractAlgorithm import algorithm.AbstractAlgorithm
import algorithm.RoomBypassingAlgorithm import algorithm.RoomBypassingAlgorithm
@@ -189,7 +188,7 @@ object DebugClInterface {
when (command) { when (command) {
"reset" -> return null "reset" -> return null
"done" -> { "done" -> {
val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray()) val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, {1}), 0, SonarRequest.Smoothing.NONE)
return sonarBuilder.build() return sonarBuilder.build()
} }
else -> { else -> {
server/src/main/java/algorithm/AbstractAlgorithm.kt
+20 -26
View File
@@ -4,6 +4,7 @@ import CodedOutputStream
import Exceptions.InactiveCarException import Exceptions.InactiveCarException
import RouteMetricRequest import RouteMetricRequest
import SonarRequest import SonarRequest
import algorithm.geometry.AngleData
import io.netty.buffer.Unpooled import io.netty.buffer.Unpooled
import io.netty.handler.codec.http.* import io.netty.handler.codec.http.*
import objects.Car import objects.Car
@@ -24,7 +25,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
private var prevState: CarState? = null private var prevState: CarState? = null
private var prevSonarDistances = mapOf<Int, Double>() private var prevSonarDistances = mapOf<Int, AngleData>()
private val defaultAngles = listOf(0, 60, 90, 120, 180).toIntArray() private val defaultAngles = listOf(0, 60, 90, 120, 180).toIntArray()
protected var requiredAngles = defaultAngles protected var requiredAngles = defaultAngles
@@ -34,43 +35,36 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
OUTER OUTER
} }
protected fun getData(angles: IntArray): DoubleArray { protected fun getData(angles: IntArray): IntArray {
val copyElemCount = 1 val attempts = 1
val anglesCoped = IntArray(angles.size * copyElemCount, { angles[it / copyElemCount] }) val threshold = 0
val smoothing = SonarRequest.Smoothing.NONE
val message = SonarRequest.BuilderSonarRequest(anglesCoped).build() val message = SonarRequest.BuilderSonarRequest(
angles = angles,
attempts = IntArray(angles.size, { attempts }),
threshold = threshold,
smoothing = smoothing)
.build()
val requestBytes = ByteArray(message.getSizeNoTag()) val requestBytes = ByteArray(message.getSizeNoTag())
message.writeTo(CodedOutputStream(requestBytes)) message.writeTo(CodedOutputStream(requestBytes))
val request = getDefaultHttpRequest(thisCar.host, sonarUrl, requestBytes) val request = getDefaultHttpRequest(thisCar.host, sonarUrl, requestBytes)
try { try {
car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf(Pair("angles", anglesCoped))) car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf(Pair("angles", angles)))
} catch (e: InactiveCarException) { } catch (e: InactiveCarException) {
println("connection error!") println("connection error!")
} }
try { try {
val distances = exchanger.exchange(IntArray(0), 300, TimeUnit.SECONDS) val distances = exchanger.exchange(IntArray(0), 300, TimeUnit.SECONDS)
val result = DoubleArray(angles.size) return distances
for (i in 0..result.size - 1) {
val distancesOnCurrentAngle = distances.drop(i * copyElemCount).take(copyElemCount)
var sum = 0
for (distance in distancesOnCurrentAngle) {
if (distance != -1) {
sum += distance
}
}
if (sum != 0) {
result[i] = (sum.toDouble()) / (copyElemCount)
}
}
return result
} catch (e: InterruptedException) { } catch (e: InterruptedException) {
println("don't have response from car!") println("don't have response from car!")
} catch (e: TimeoutException) { } catch (e: TimeoutException) {
println("don't have response from car. Timeout!") println("don't have response from car. Timeout!")
} }
return DoubleArray(0) return IntArray(0)
} }
protected fun moveCar(message: RouteMetricRequest) { protected fun moveCar(message: RouteMetricRequest) {
@@ -103,12 +97,12 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
if (distances.size != angles.size) { if (distances.size != angles.size) {
throw RuntimeException("error! angles and distances have various sizes") throw RuntimeException("error! angles and distances have various sizes")
} }
val anglesDistances = mutableMapOf<Int, Double>() val anglesDistances = mutableMapOf<Int, AngleData>()
for (i in 0..angles.size - 1) { for (i in 0..angles.size - 1) {
if (Math.abs(distances[i]) < 0.01) { if (Math.abs(distances[i]) < 0.01) {
continue continue
} }
anglesDistances.put(angles[i], distances[i]) anglesDistances.put(angles[i], AngleData(angles[i], distances[i]))
} }
this.requiredAngles = defaultAngles this.requiredAngles = defaultAngles
@@ -133,7 +127,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
return prevState return prevState
} }
protected fun getPrevSonarDistances(): Map<Int, Double> { protected fun getPrevSonarDistances(): Map<Int, AngleData> {
return prevSonarDistances return prevSonarDistances
} }
@@ -141,8 +135,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
return requiredAngles return requiredAngles
} }
protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState? protected abstract fun getCarState(anglesDistances: Map<Int, AngleData>): CarState?
protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest protected abstract fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest
protected abstract fun afterGetCommand(route: RouteMetricRequest) protected abstract fun afterGetCommand(route: RouteMetricRequest)
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+38 -46
View File
@@ -1,15 +1,14 @@
package algorithm package algorithm
import algorithm.geometry.Line
import algorithm.geometry.Vector
import objects.Car import objects.Car
import RouteMetricRequest import RouteMetricRequest
import algorithm.geometry.*
import java.util.concurrent.Exchanger import java.util.concurrent.Exchanger
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) { class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
public var wallAngleWithOX = 0.0//in radian var wallAngleWithOX = 0.0//in radian
public var wallLength = 0.0//sm var wallLength = 0.0//sm
var errorCount = 0 var errorCount = 0
var carX = 0 var carX = 0
@@ -18,17 +17,18 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
private var prevPoint = Pair(0.0, 0.0) private var prevPoint = Pair(0.0, 0.0)
override fun getCarState(anglesDistances: Map<Int, Double>): CarState? { override fun getCarState(anglesDistances: Map<Int, AngleData>): CarState? {
val dist0 = anglesDistances[0] val angleData0 = anglesDistances[0]
val dist90 = anglesDistances[90] val angleData90 = anglesDistances[90]
if (dist90 == null || dist90 > 85) { if (angleData90 == null || angleData90.distance > 85) {
requiredAngles = getIntArray(0, 60, 90, 120, 180) requiredAngles = getIntArray(0, 60, 90, 120, 180)
return CarState.OUTER return CarState.OUTER
} }
val dist90 = angleData90.distance
if (dist90 < 20) { if (dist90 < 20) {
return CarState.WALL return CarState.WALL
} }
if (dist0 == null || dist0 > 70) { if (angleData0 == null || angleData0.distance > 70) {
return CarState.WALL return CarState.WALL
} }
return CarState.INNER return CarState.INNER
@@ -59,14 +59,14 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
} }
} }
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest { override fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest {
val dist0 = anglesDistances[0] val angleData0 = anglesDistances[0]
val dist60 = anglesDistances[60] val angleData60 = anglesDistances[60]
val dist90 = anglesDistances[90] val angleData90 = anglesDistances[90]
val dist120 = anglesDistances[120] val angleData120 = anglesDistances[120]
val dist180 = anglesDistances[180] val angleData180 = anglesDistances[180]
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0)) val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
if (dist120 == null || dist90 == null || dist60 == null) { if (angleData120 == null || angleData90 == null || angleData60 == null) {
println("null distance!") println("null distance!")
if (errorCount >= 3) { if (errorCount >= 3) {
errorCount = 0 errorCount = 0
@@ -79,26 +79,26 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
} }
if (getPrevState() == null) { if (getPrevState() == null) {
//its first run, save right wall //its first run, save right wall
RoomModel.lines.add(Line(0.0, 1.0, dist90)) RoomModel.lines.add(Line(0.0, 1.0, angleData90.distance.toDouble()))
prevPoint = Pair(0.0, -dist90) prevPoint = Pair(0.0, -angleData90.distance.toDouble())
if (dist0 != null) { if (angleData0 != null) {
wallLength = dist0 wallLength = angleData0.distance.toDouble()
} }
} }
when (state) { when (state) {
CarState.WALL -> { CarState.WALL -> {
if (wallLength.toInt() == 0 && (dist0 != null && dist180 != null)) { if (wallLength.toInt() == 0 && (angleData0 != null && angleData180 != null)) {
wallLength = dist0 + dist180 wallLength = angleData0.distance.toDouble() + angleData180.distance.toDouble()
} }
if (dist90 > 40 || dist90 < 20) { if (angleData90.distance > 40 || angleData90.distance < 20) {
val rotationDirection = if (dist90 > 40) RIGHT else LEFT val rotationDirection = if (angleData90.distance > 40) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setDistances(getIntArray(10, 35)) resultBuilder.setDistances(getIntArray(10, 35))
return resultBuilder.build() return resultBuilder.build()
} }
if (Math.abs(dist120 - dist60) > 10) { if (Math.abs(angleData120.distance - angleData60.distance) > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT val rotationDirection = if (angleData120.distance > angleData60.distance) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection)) resultBuilder.setDirections(getIntArray(rotationDirection))
resultBuilder.setDistances(getIntArray(15)) resultBuilder.setDistances(getIntArray(15))
return resultBuilder.build() return resultBuilder.build()
@@ -111,18 +111,18 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
CarState.INNER -> { CarState.INNER -> {
//в угле первое действие - встать максимально паралельно стене. //в угле первое действие - встать максимально паралельно стене.
if (Math.abs(dist120 - dist60) > 10) { if (Math.abs(angleData120.distance - angleData60.distance) > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT val rotationDirection = if (angleData120.distance > angleData60.distance) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection)) resultBuilder.setDirections(getIntArray(rotationDirection))
resultBuilder.setDistances(getIntArray(12)) resultBuilder.setDistances(getIntArray(12))
return resultBuilder.build() return resultBuilder.build()
} }
//если стоим паралельно - можно мерить угол //если стоим паралельно - можно мерить угол
val pointsNextLine = arrayListOf<Pair<Double, Double>>() val pointsNextLine = arrayListOf<Point>()
for (angleDist in anglesDistances) { for (angleData in anglesDistances.values) {
if (angleDist.key <= 15) { if (angleData.angle <= 15) {
pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair())) pointsNextLine.add(angleData.toPoint(Util.radianToDegrees(wallAngleWithOX)))
} }
} }
if (pointsNextLine.size < 3) { if (pointsNextLine.size < 3) {
@@ -178,30 +178,22 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
return angle * 180 / Math.PI return angle * 180 / Math.PI
} }
private fun sonarAngleDistToPoint(angleDist: Pair<Int, Double>): Pair<Double, Double> {
val angle = wallAngleWithOX - degreesToRadian(angleDist.first) private fun approximatePointsByLine(points: Array<Point>): Line {
val dist = angleDist.second
val result = Pair(Math.cos(angle) * dist, Math.sin(angle) * dist)
return result
}
private fun approximatePointsByLine(points: Array<Pair<Double, Double>>): Line {
// val p1 = points.first() // val p1 = points.first()
// val p2 = points[1] // val p2 = points[1]
// return Line(p2.second - p1.second, p1.first - p2.first, -p1.first * p2.second + p1.second * p2.first) // return Line(p2.second - p1.second, p1.first - p2.first, -p1.first * p2.second + p1.second * p2.first)
val sumX = points.sumByDouble { it.first } val sumX = points.sumByDouble { it.x }
val sumXQuad = points.sumByDouble { it.first * it.first } val sumXQuad = points.sumByDouble { it.x * it.x }
val sumY = points.sumByDouble { it.second } val sumY = points.sumByDouble { it.y }
val sumXY = points.sumByDouble { it.second * it.first } val sumXY = points.sumByDouble { it.y * it.x }
val pointsCount = points.size val pointsCount = points.size
val den = pointsCount * sumXQuad - sumX * sumX val den = pointsCount * sumXQuad - sumX * sumX
if (Math.abs(den) < 0.001) { if (Math.abs(den) < 0.001) {
return Line(1.0, 0.0, -points.first().first) return Line(1.0, 0.0, -points.first().x)
} }
val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX) val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
val b = (sumY - sumX * k) / pointsCount val b = (sumY - sumX * k) / pointsCount
server/src/main/java/algorithm/RoomModel.kt
+5 -14
View File
@@ -35,23 +35,14 @@ object RoomModel {
val end_y = IntArray(algorithm.points.size) val end_y = IntArray(algorithm.points.size)
for (i in 0..algorithm.points.size - 1) { for (i in 0..algorithm.points.size - 2) {
val curPoint = algorithm.points[i] val curPoint = algorithm.points[i]
if (algorithm.points.size - 1 == i) {
// val nextPoint = algorithm.points[0]
// begin_x[i] = curPoint.first.toInt()
// begin_y[i] = curPoint.second.toInt()
//
// end_x[i] = nextPoint.first.toInt()
// end_y[i] = nextPoint.second.toInt()
continue
}
val nextPoint = algorithm.points[i + 1] val nextPoint = algorithm.points[i + 1]
begin_x[i] = curPoint.first.toInt() begin_x[i] = (curPoint.x + 0.5).toInt()
begin_y[i] = curPoint.second.toInt() begin_y[i] = (curPoint.y + 0.5).toInt()
end_x[i] = nextPoint.first.toInt() end_x[i] = (nextPoint.x + 0.5).toInt()
end_y[i] = nextPoint.second.toInt() end_y[i] = (nextPoint.y + 0.5).toInt()
} }
return Waypoints.BuilderWaypoints(begin_x, begin_y, end_x, end_y, false).build() return Waypoints.BuilderWaypoints(begin_x, begin_y, end_x, end_y, false).build()
} }
server/src/main/java/algorithm/RoomTest.kt
+15 -10
View File
@@ -3,20 +3,23 @@ package algorithm
import objects.Car import objects.Car
import java.util.concurrent.Exchanger import java.util.concurrent.Exchanger
import RouteMetricRequest import RouteMetricRequest
import algorithm.geometry.AngleData
import algorithm.geometry.Point
class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) { class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
val points = arrayListOf<Pair<Double, Double>>() val points = arrayListOf<Point>()
private val DISTANCE_TO_WALL_THRESHOLD = 60
var carX = 0 var carX = 0
var carY = 0 var carY = 0
var carAngle = 0 var carAngle = 0
override fun getCarState(anglesDistances: Map<Int, Double>): CarState { override fun getCarState(anglesDistances: Map<Int, AngleData>): CarState {
return CarState.WALL return CarState.WALL
} }
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest { override fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest {
val dist0 = anglesDistances[0] val dist0 = anglesDistances[0]
val dist60 = anglesDistances[60] val dist60 = anglesDistances[60]
val dist90 = anglesDistances[90] val dist90 = anglesDistances[90]
@@ -28,27 +31,29 @@ class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm
return resultBuilder.build() return resultBuilder.build()
} }
val sonarAngle = carAngle - 90 val sonarAngle = carAngle - 90
points.add(Pair(carX + dist90 * Math.cos(degreesToRadian(sonarAngle)), points.add(Point(
carY + dist90 * Math.sin(degreesToRadian(sonarAngle)))) x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
))
if (dist120 == null || dist60 == null) { if (dist120 == null || dist60 == null) {
resultBuilder.setDirections(getIntArray(FORWARD)) resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setDistances(getIntArray(10)) resultBuilder.setDistances(getIntArray(10))
return resultBuilder.build() return resultBuilder.build()
} }
if (dist0 != null && dist0 < 60) { if (dist0 != null && dist0.distance < DISTANCE_TO_WALL_THRESHOLD) {
resultBuilder.setDirections(getIntArray(LEFT, FORWARD)) resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
resultBuilder.setDistances(getIntArray(20, 10)) resultBuilder.setDistances(getIntArray(20, 10))
return resultBuilder.build() return resultBuilder.build()
} }
val abs = Math.abs(dist120 - dist60) val abs = Math.abs(dist120.distance - dist60.distance)
if (abs > 10) { if (abs > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT val rotationDirection = if (dist120.distance > dist60.distance) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
return resultBuilder.build() return resultBuilder.build()
} }
if (dist90 > 50 || dist90 < 25) { if (dist90.distance > 50 || dist90.distance < 25) {
val rotationDirection = if (dist90 > 50) RIGHT else LEFT val rotationDirection = if (dist90.distance > 50) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD)) resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setDistances(getIntArray(10, 10)) resultBuilder.setDistances(getIntArray(10, 10))
return resultBuilder.build() return resultBuilder.build()
server/src/main/java/algorithm/geometry/AngleData.kt
+19
View File
@@ -0,0 +1,19 @@
package algorithm.geometry
class AngleData(val angle: Int, val distance: Int) {
fun toPoint(carAngleOX: Int): Point {
//convert to global coordinate system
val angle = carAngleOX - angle
val radianAngle = Util.degreesToRadian(angle)
return Point(
x = Math.cos(radianAngle) * distance,
y = Math.sin(radianAngle) * distance
)
}
}
server/src/main/java/algorithm/geometry/Point.kt
+4
View File
@@ -0,0 +1,4 @@
package algorithm.geometry
class Point(val x: Double, val y: Double) {
}
server/src/main/java/algorithm/geometry/Util.kt
+14
View File
@@ -0,0 +1,14 @@
package algorithm.geometry
object Util {
fun degreesToRadian(angleDegrees: Int): Double {
return Math.PI * angleDegrees / 180.0
}
fun radianToDegrees(angleRad: Double): Int {
return (180.0 * angleRad / Math.PI).toInt()
}
}