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