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

upgrade algorithm

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-24 18:01:07 +03:00
parent 0102312480
commit 4c43c3c411
3 changed files with 135 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
server/src/main/java/algorithm/AbstractAlgorithm.kt
+13 -1
View File
@@ -51,7 +51,16 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
val result = DoubleArray(angles.size)
for (i in 0..result.size - 1) {
//todo sonar can send -1!!!
result[i] = distances.drop(i * copyElemCount).take(copyElemCount).sum().toDouble() / copyElemCount
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) {
@@ -90,6 +99,9 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
}
val anglesDistances = mutableMapOf<Int, Double>()
for (i in 0..angles.size - 1) {
if (Math.abs(distances[i]) < 0.01) {
continue
}
anglesDistances.put(angles[i], distances[i])
}
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+106 -17
View File
@@ -2,6 +2,7 @@ package algorithm
import RouteRequest
import objects.Car
import java.util.*
import java.util.concurrent.Exchanger
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
@@ -10,9 +11,22 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
private val ROTATION_VELOCITY = 11.0//degrees/s
private var wallLength = 0.0//in sm
private var wallAngle = 0.0//in radian
private var xPos = 0.0
private var yPos = 0.0
override fun getCarState(anglesDistances: Map<Int, Double>): CarState {
val dist0 = anglesDistances[0]!!
val dist90 = anglesDistances[90]!!
val dist60 = anglesDistances[60]!!
val oldDist90 = getPrevSonarDistances()[90]
println(dist90)
println(oldDist90)
if (oldDist90 != null && oldDist90 + 60 < dist90) {
return CarState.OUTER
}
if (dist90 < 20) {
return CarState.WALL
}
@@ -36,33 +50,108 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
val dist90 = anglesDistances[90]!!
val dist120 = anglesDistances[120]!!
val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
if (state == CarState.WALL) {
if (dist90 > 40 || dist90 < 20) {
val rotationDirection = if (dist90 > 40) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setTimes(getIntArray(2000, 1000))
return resultBuilder.build()
}
when (state) {
CarState.WALL -> {
if (dist90 > 40 || dist90 < 20) {
val rotationDirection = if (dist90 > 40) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
wallLength += 1 * MOVE_VELOCITY
resultBuilder.setTimes(getIntArray(2000, 1000))
return resultBuilder.build()
}
if (Math.abs(dist120 - dist60) > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection))
if (Math.abs(dist120 - dist60) > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection))
resultBuilder.setTimes(getIntArray(1000))
return resultBuilder.build()
}
resultBuilder.setDirections(getIntArray(FORWARD))
wallLength += 1 * MOVE_VELOCITY
resultBuilder.setTimes(getIntArray(1000))
return resultBuilder.build()
}
CarState.INNER -> {
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setTimes(getIntArray(1000))
return resultBuilder.build()
if (getPrevState() != state) {
val pointsNextLine = arrayListOf<Pair<Double, Double>>()
val pointsCurrentLine = arrayListOf<Pair<Double, Double>>()
for (angleDist in anglesDistances) {
if (angleDist.key <= 40) {
pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair()))
} else if (angleDist.key <= 120) {
pointsCurrentLine.add(sonarAngleDistToPoint(angleDist.toPair()))
}
}
val innerAnglePoint = getInnerAnglePoint(pointsNextLine, pointsCurrentLine)
val vectorNextLine = Vector(innerAnglePoint.first, innerAnglePoint.second,
pointsNextLine[0].first, pointsNextLine[0].second)
} else {
resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT))
resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt()))
return resultBuilder.build()
val vectorCurrentLine = Vector(innerAnglePoint.first, innerAnglePoint.second,
pointsCurrentLine.last().first, pointsCurrentLine.last().second)
val scalarMult = vectorCurrentLine.scalarProduct(vectorNextLine)
val angle = Math.acos(scalarMult / (vectorCurrentLine.length() * vectorNextLine.length()))
wallAngle += angle
//todo add length and save this point
}
resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT))
resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt()))
return resultBuilder.build()
}
CarState.OUTER -> {
resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT))
resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt()))
return resultBuilder.build()
}
}
}
private fun getInnerAnglePoint(pointsNextLine: ArrayList<Pair<Double, Double>>,
pointsCurrentLine: ArrayList<Pair<Double, Double>>): Pair<Double, Double> {
val nextLine = approximatePointsByLine(pointsNextLine.toTypedArray())
val currentLine = approximatePointsByLine(pointsCurrentLine.toTypedArray())
val b1 = currentLine.second
val k1 = currentLine.first
val b2 = nextLine.second
val k2 = nextLine.first
val intersectionPoint = Pair((b1 - b2) / (k2 - k1), (k2 * b1 - b2 * k1) / (k2 - k1))
return intersectionPoint
}
private fun degreesToRadian(angle: Int): Double {
return Math.PI * angle / 180
}
private fun sonarAngleDistToPoint(angleDist: Pair<Int, Double>): Pair<Double, Double> {
val angle = wallAngle - degreesToRadian(angleDist.first)
val dist = angleDist.second
val result = Pair(xPos + Math.cos(angle) * dist, yPos + Math.sin(angle) * dist)
return result
}
//returns coef in y=kx+b (first is k, second is b)
private fun approximatePointsByLine(points: Array<Pair<Double, Double>>): Pair<Double, Double> {
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 pointsCount = points.size
val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
val b = (sumY - sumX * k) / pointsCount
return Pair(k, b)
}
private fun calculateAngleWithWall(anglesDistances: MutableMap<Int, Double>): Double {
val dist60 = anglesDistances[60]!!
val dist120 = anglesDistances[120]!!
server/src/main/java/algorithm/Vector.kt
+16
View File
@@ -0,0 +1,16 @@
package algorithm
class Vector constructor(val x: Double, val y: Double) {
constructor(x1: Double, y1: Double, x2: Double, y2: Double) : this(x2 - x1, y2 - y1)
fun scalarProduct(vector: Vector): Double {
return this.x * vector.x + this.y * vector.y
}
fun length(): Double {
return Math.sqrt(x * x + y * y)
}
}