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

alg

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-26 15:05:07 +03:00
parent fbe6081d5d
commit be7c7f7876
5 changed files with 46 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt
+1 -1
View File
@@ -123,7 +123,7 @@ class ControllerEmulator : Controller {
continue
}
val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0)
+ Math.pow(yIntersection - ySensor0, 2.0)) * 1000000);
+ Math.pow(yIntersection - ySensor0, 2.0)) * 1000000)
if (currentDistance < result) {
result = currentDistance
}
server/build.gradle
+6
View File
@@ -30,8 +30,14 @@ task getDeps(type: Copy) {
from sourceSets.main.compileClasspath
into 'build/libs'
}
task compileProto() {
exec {
executable './../proto/protofiles_sources/compile_proto.sh'
}
}
build.dependsOn getDeps
build.dependsOn compileProto
jar {
manifest {
server/src/main/java/DebugClInterface.kt
+3 -1
View File
@@ -22,6 +22,8 @@ object DebugClInterface {
"refloc - refresh all car locations\n" +
"sonar [car_id] - get sonar data\n" +
"dbinfo [car_id] [type] - refresh all car locations\n." +
"lines - print lines, detected by car\n" +
"alg [count] - run algorithm. Make [count] iteration\n" +
"type is string name of value or int value. available values: MEMORYSTATS - 0\n" +
"stop - exit from this interface and stop all threads\n"
@@ -57,7 +59,7 @@ object DebugClInterface {
"refloc" -> executeRefreshLocationCommand()
"sonar" -> executeSonarCommand(readString)
"dbinfo" -> executeDebugInfoCommand(readString)
"points" -> println(algorithm.RoomModel.lines)
"lines" -> println(algorithm.RoomModel.lines)
"pos" -> {
val tmp = algorithmImpl
if (tmp is RoomBypassingAlgorithm) {
server/src/main/java/algorithm/AbstractAlgorithm.kt
+13 -6
View File
@@ -21,9 +21,11 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
protected val LEFT = 2
protected val RIGHT = 3
private var prevState:CarState? = null
private var prevState: CarState? = null
private var prevSonarDistances = mapOf<Int, Double>()
private val defaultAngles = listOf(0, 60, 90, 120, 180).toIntArray()
protected var requiredAngles = defaultAngles
protected enum class CarState {
WALL,
@@ -104,10 +106,13 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
anglesDistances.put(angles[i], distances[i])
}
val state = getCarState(anglesDistances)
this.requiredAngles = defaultAngles
val state = getCarState(anglesDistances)
if (state == null) {
return
}
val command = getCommand(anglesDistances, state)
calculateCarPosition(command, state)
println(Arrays.toString(command.directions))
println(Arrays.toString(command.times))
@@ -126,10 +131,12 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
return prevSonarDistances
}
protected abstract fun calculateCarPosition(route: RouteRequest, state: CarState)
protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState
private fun getAngles(): IntArray {
return requiredAngles
}
protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState?
protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest
protected abstract fun getAngles(): IntArray
private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest {
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+23 -33
View File
@@ -14,17 +14,17 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
public var wallAngleWithOX = 0.0//in radian
public var wallLength = 0.0//sm
var xPos = 0.0
var yPos = 0.0
var carAngle = 0.0
var errorCount = 0
private var prevPoint = Pair(0.0, 0.0)
override fun getCarState(anglesDistances: Map<Int, Double>): CarState {
override fun getCarState(anglesDistances: Map<Int, Double>): CarState? {
val dist0 = anglesDistances[0]
val dist90 = anglesDistances[90]
if (dist90 == null || dist90 > 85) {
//best analysis of outer angle is check minimum in values 60,70,80,90,100,...
requiredAngles = getIntArray(0, 60, 90, 120, 180)
return CarState.OUTER
}
if (dist90 < 20) {
@@ -40,33 +40,6 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
return args
}
override fun getAngles(): IntArray {
return IntArray(37, { it * 5 })
// return IntArray(181, { it * 1 })
}
override fun calculateCarPosition(route: RouteRequest, state: CarState) {
//todo for best values make calculate carAngle from right wall
// for (i in 0..route.directions.size - 1) {
// when (route.directions[i]) {
// FORWARD -> {
// xPos += route.times[i] * MOVE_VELOCITY * Math.cos(carAngle)
// yPos += route.times[i] * MOVE_VELOCITY * Math.sin(carAngle)
// }
// LEFT -> {
// carAngle += degreesToRadian((route.times[i] * ROTATION_VELOCITY).toInt())
// }
// RIGHT -> {
// carAngle -= degreesToRadian((route.times[i] * ROTATION_VELOCITY).toInt())
// }
// }
// }
// if (state == CarState.INNER) {
// carAngle = wallAngleWithOX
// }
}
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
val dist0 = anglesDistances[0]
val dist60 = anglesDistances[60]
@@ -76,6 +49,13 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
if (dist120 == null || dist90 == null || dist60 == null) {
println("null distance!")
if (errorCount >= 3) {
errorCount = 0
resultBuilder.setDirections(getIntArray(BACKWARD))
resultBuilder.setTimes(getIntArray((15.0 / MOVE_VELOCITY).toInt()))
return resultBuilder.build()
}
errorCount++
return resultBuilder.build()
}
if (getPrevState() == null) {
@@ -107,7 +87,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
}
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setTimes(getIntArray((25.0 / MOVE_VELOCITY).toInt()))
resultBuilder.setTimes(getIntArray((35.0 / MOVE_VELOCITY).toInt()))
return resultBuilder.build()
}
CarState.INNER -> {
@@ -127,6 +107,10 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair()))
}
}
if (pointsNextLine.size < 3) {
requiredAngles = IntArray(37, { it * 5 })
return resultBuilder.build()
}
val nextLine = approximatePointsByLine(pointsNextLine.toTypedArray())
val currentLine = RoomModel.lines.last()
@@ -147,7 +131,9 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
val vectorCurrentLine = Vector(xIntersection, yIntersection, prevPoint.first, prevPoint.second)
val scalarProduct = vectorCurrentLine.scalarProduct(vectorNextLine)
val angle = Math.acos(scalarProduct / (vectorCurrentLine.length() * vectorNextLine.length()))
// val angle = Math.acos(scalarProduct / (vectorCurrentLine.length() * vectorNextLine.length()))
//todo
val angle = Math.PI / 2
val wallsAngleInDegrees = radiansToDegrees(angle)
wallAngleWithOX += degreesToRadian(180 - wallsAngleInDegrees.toInt())
prevPoint = Pair(xIntersection, yIntersection)
@@ -197,6 +183,10 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
val sumXY = points.sumByDouble { it.second * it.first }
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)
}
val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
val b = (sumY - sumX * k) / pointsCount