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

working algorithm on inner angles

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-29 15:06:15 +03:00
parent 5a1b94f3c7
commit f7b164b9fb
10 changed files with 295 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
server/src/main/java/algorithm/AbstractAlgorithm.kt
+33 -17
View File
@@ -4,6 +4,7 @@ import CodedOutputStream
import Exceptions.InactiveCarException
import RouteMetricRequest
import SonarRequest
import algorithm.geometry.Angle
import algorithm.geometry.AngleData
import io.netty.buffer.Unpooled
import io.netty.handler.codec.http.*
@@ -17,6 +18,10 @@ import java.util.concurrent.TimeoutException
abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntArray>) {
open val ATTEMPTS = 1
open val THRESHOLD = 0
open val SMOOTHING = SonarRequest.Smoothing.NONE
protected val FORWARD = 0
protected val BACKWARD = 1
protected val LEFT = 2
@@ -24,8 +29,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
private var prevState: CarState? = null
private var prevSonarDistances = mapOf<Int, AngleData>()
private val defaultAngles = listOf(0, 60, 90, 120, 180).toIntArray()
private var prevSonarDistances = mapOf<Angle, AngleData>()
private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(90), Angle(120), Angle(180))
protected var requiredAngles = defaultAngles
protected enum class CarState {
@@ -34,23 +39,20 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
OUTER
}
protected fun getData(angles: IntArray): IntArray {
val attempts = 1
val threshold = 0
val smoothing = SonarRequest.Smoothing.NONE
protected fun getData(angles: Array<Angle>): IntArray {
val anglesIntArray = (angles.map { it -> it.degs() }).toIntArray()
val message = SonarRequest.BuilderSonarRequest(
angles = angles,
attempts = IntArray(angles.size, { attempts }),
windowSize = 0,
smoothing = smoothing)
angles = anglesIntArray,
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", angles)))
car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf(Pair("angles", anglesIntArray)))
} catch (e: InactiveCarException) {
println("connection error!")
}
@@ -91,12 +93,15 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
}
fun iterate() {
if (RoomModel.finished) {
return
}
val angles = getAngles()
val distances = getData(angles)
if (distances.size != angles.size) {
throw RuntimeException("error! angles and distances have various sizes")
}
val anglesDistances = mutableMapOf<Int, AngleData>()
val anglesDistances = mutableMapOf<Angle, AngleData>()
for (i in 0..angles.size - 1) {
if (Math.abs(distances[i]) < 0.01) {
continue
@@ -106,7 +111,10 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
this.requiredAngles = defaultAngles
val state = getCarState(anglesDistances) ?: return
val state = getCarState(anglesDistances)
if (state == null) {
return
}
val command = getCommand(anglesDistances, state)
afterGetCommand(command)
println(Arrays.toString(command.directions))
@@ -119,12 +127,20 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
}
private fun getAngles(): IntArray {
protected fun getPrevState(): CarState? {
return prevState
}
protected fun getPrevSonarDistances(): Map<Angle, AngleData> {
return prevSonarDistances
}
private fun getAngles(): Array<Angle> {
return requiredAngles
}
protected abstract fun getCarState(anglesDistances: Map<Int, AngleData>): CarState?
protected abstract fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest
protected abstract fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState?
protected abstract fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest
protected abstract fun afterGetCommand(route: RouteMetricRequest)
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+111 -40
View File
@@ -1,69 +1,140 @@
package algorithm
import objects.Car
import java.util.concurrent.Exchanger
import RouteMetricRequest
import algorithm.geometry.Angle
import algorithm.geometry.AngleData
import algorithm.geometry.Point
import algorithm.geometry.Wall
import objects.Car
import java.util.concurrent.Exchanger
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
val points = arrayListOf<Point>()
private val DISTANCE_TO_WALL_THRESHOLD = 60
// TODO: set to appropriate values
override val THRESHOLD = 0
override val ATTEMPTS: Int = 3
override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
private val MAX_DISTANCE_TO_WALL_AHEAD = 60
private val ISOSCALENESS_THRESHOLD = 10
private val DISTANCE_TO_WALL_UPPER_BOUND = 50
private val DISTANCE_TO_WALL_LOWER_BOUND = 25
var carX = 0
var carY = 0
var carAngle = 0
override fun getCarState(anglesDistances: Map<Int, AngleData>): CarState {
override fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState {
return CarState.WALL
}
override fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest {
val dist0 = anglesDistances[0]
val dist60 = anglesDistances[60]
val dist90 = anglesDistances[90]
val dist120 = anglesDistances[120]
private fun noParallelWallFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
if (dist90 == null) {
resultBuilder.setDirections(getIntArray(RIGHT))
resultBuilder.setDistances(getIntArray(10))
return resultBuilder.build()
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setDistances(getIntArray(10))
return resultBuilder.build()
}
private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
resultBuilder.setDirections(getIntArray(RIGHT))
resultBuilder.setDistances(getIntArray(10))
return resultBuilder.build()
}
private fun wallAheadFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
resultBuilder.setDirections(getIntArray(LEFT))
resultBuilder.setDistances(getIntArray(calculateAngle(anglesDistances, state).degs()))
addWall()
return resultBuilder.build()
}
private fun alignParallelToWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
// we're sure that we have 120 and 60 degs measurements as they were checked before
val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
return resultBuilder.build()
}
private fun correctDistanceToParallelWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
val rotationDirection = if (anglesDistances[Angle(90)]!!.distance > 50) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setDistances(getIntArray(10, 10))
return resultBuilder.build()
}
private fun addWall() {
RoomModel.updateWalls()
if (RoomModel.walls.last() == RoomModel.walls.first() && RoomModel.walls.size > 1) {
println("YEAH!")
RoomModel.finished = true
RoomModel.walls.removeAt(RoomModel.walls.size - 1)
}
RoomModel.walls.add(Wall())
}
override fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
val dist0 = anglesDistances[Angle(0)]
val dist60 = anglesDistances[Angle(60)]
val dist90 = anglesDistances[Angle(90)]
val dist120 = anglesDistances[Angle(120)]
val sonarAngle = carAngle - 90
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.distance < DISTANCE_TO_WALL_THRESHOLD) {
resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
resultBuilder.setDistances(getIntArray(20, 10))
return resultBuilder.build()
}
val abs = Math.abs(dist120.distance - dist60.distance)
if (abs > 10) {
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.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()
// Check most basic measurements: 60/90/120
if (dist90 == null) {
return noOrthogonalMeasurementFound(anglesDistances, state)
}
if (dist120 == null || dist60 == null) {
return noParallelWallFound(anglesDistances, state)
}
// Add point to room map
val point = Point(
x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
)
RoomModel.walls.last().addPoint(point)
// Check if corner reached
if (dist0 != null && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) {
return wallAheadFound(anglesDistances, state)
}
// Checks for paralleleness
// Check if wall is not parallel
val diff = Math.abs(dist120.distance - dist60.distance)
if (diff > ISOSCALENESS_THRESHOLD) {
return alignParallelToWall(anglesDistances, state)
}
// Check if wall is too close or too far
if (dist90.distance > 50 || dist90.distance < 25) {
return correctDistanceToParallelWall(anglesDistances, state)
}
// default case: everything is ok, just move forward
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setDistances(getIntArray(20))
return resultBuilder.build()
}
private fun calculateAngle(anglesDistances: Map<Angle, AngleData>, state: CarState): Angle {
// TODO: stub here, make proper angle calculation
return Angle(90)
}
private fun getIntArray(vararg args: Int): IntArray {
return args
}
server/src/main/java/algorithm/RoomModel.kt
+29 -12
View File
@@ -4,11 +4,16 @@ import DebugClInterface
import DebugResponse
import Waypoints
import algorithm.geometry.Line
import algorithm.geometry.Point
import algorithm.geometry.Wall
import objects.Car
import java.util.*
object RoomModel {
val lines = arrayListOf<Line>()
val walls = arrayListOf<Wall>(Wall())
var finished = false
val linesModel = listOf(Line(0.0, 1.0, -300.0),
Line(1.0, 0.0, 150.0),
@@ -23,21 +28,17 @@ object RoomModel {
var currentPosition_y = 0
fun getUpdate(): Waypoints {
val algorithm = DebugClInterface.algorithmImpl
if (algorithm == null || algorithm !is RoomBypassingAlgorithm) {
val emptyArr = IntArray(0)
return Waypoints.BuilderWaypoints(emptyArr, emptyArr, emptyArr, emptyArr, false).build()
val points = getWallsPoints()
}
val begin_x = IntArray(algorithm.points.size)
val begin_y = IntArray(algorithm.points.size)
val end_x = IntArray(algorithm.points.size)
val end_y = IntArray(algorithm.points.size)
val begin_x = IntArray(points.size)
val begin_y = IntArray(points.size)
val end_x = IntArray(points.size)
val end_y = IntArray(points.size)
for (i in 0..algorithm.points.size - 2) {
val curPoint = algorithm.points[i]
val nextPoint = algorithm.points[i + 1]
for (i in 0..points.size - 2) {
val curPoint = points[i]
val nextPoint = points[i + 1]
begin_x[i] = (curPoint.x + 0.5).toInt()
begin_y[i] = (curPoint.y + 0.5).toInt()
@@ -47,6 +48,22 @@ object RoomModel {
return Waypoints.BuilderWaypoints(begin_x, begin_y, end_x, end_y, false).build()
}
private fun getWallsPoints(): Array<Point> {
return walls.flatMap({ it.points }).toTypedArray()
}
fun updateWalls() {
if (walls.size < 2) {
// no walls to intersect
return
}
val line1 = walls.last().line
val line2 = walls[walls.size - 2].line
val intersection: Point = line1.intersect(line2)
walls[walls.size - 2].addPoint(intersection)
}
fun getDebugInfo(): DebugResponse {
val aRef = IntArray(linesModel.size)
server/src/main/java/algorithm/geometry/Angle.kt
+22
View File
@@ -0,0 +1,22 @@
package algorithm.geometry
data class Angle(private val degrees: Int) {
constructor(radians: Double) : this((180.0 * radians / Math.PI).toInt())
fun rads(): Double {
return Math.PI * degrees / 180.0
}
fun degs(): Int {
return degrees
}
operator fun minus(other: Angle): Angle {
return Angle(degrees - other.degrees)
}
operator fun plus(other: Angle): Angle {
return Angle(degrees + other.degrees)
}
}
server/src/main/java/algorithm/geometry/AngleData.kt
+3 -3
View File
@@ -1,14 +1,14 @@
package algorithm.geometry
class AngleData(val angle: Int, val distance: Int) {
class AngleData(val angle: Angle, val distance: Int) {
fun toPoint(carAngleOX: Int): Point {
fun toPoint(carAngleOX: Angle): Point {
//convert to global coordinate system
val angle = carAngleOX - angle
val radianAngle = Util.degreesToRadian(angle)
val radianAngle = angle.rads()
return Point(
x = Math.cos(radianAngle) * distance,
server/src/main/java/algorithm/geometry/DoubleComparison.kt
+13
View File
@@ -0,0 +1,13 @@
package algorithm.geometry
fun Double.lt(other: Double): Boolean {
return this - other < -Util.eps
}
fun Double.gt(other: Double): Boolean {
return this - other > Util.eps
}
fun Double.eq(other: Double): Boolean {
return Math.abs(this - other) < Util.eps;
}
server/src/main/java/algorithm/geometry/Line.kt
+41 -4
View File
@@ -1,21 +1,58 @@
package algorithm.geometry
class Line(val A: Double, val B: Double, var C: Double) {
class Line(var A: Double, var B: Double, var C: Double) {
val COMPARISON_THRESHOLD = 0.5
init {
normalize()
}
fun getIntersectionPoint(lineTwo: Line): Pair<Double, Double> {
fun intersect(lineTwo: Line): Point {
val slope = this.A * lineTwo.B - this.B * lineTwo.A
if (Math.abs(slope) < 0.001) {
throw ArithmeticException("lines is parallel")
}
val xIntersection = (this.B * lineTwo.C - lineTwo.B * this.C) / slope
val yIntersection = (this.C * lineTwo.A - lineTwo.C * this.A) / slope
return Pair(xIntersection, yIntersection)
return Point(xIntersection, yIntersection)
}
override fun toString(): String{
override fun toString(): String {
return "Line(A=$A, B=$B, C=$C)"
}
fun normalize() {
val div = Math.sqrt(A * A + B * B);
A /= div;
B /= div;
C /= div;
if (A.eq(0.0)) {
if (B.lt(0.0)) {
A *= -1
B *= -1
C *= -1
}
}
else
{
if (A.lt(0.0)){
A *= -1
B *= -1
C *= -1
}
}
}
fun metricDist(other: Line): Double {
return Math.sqrt((A - other.A) * (A - other.A) + (B - other.B) * (B - other.B) + (C - other.C) * (C - other.C))
}
override fun equals(other: Any?): Boolean {
if (other == null) return false
if (!(other is Line)) return false
return metricDist(other) < COMPARISON_THRESHOLD
}
}
server/src/main/java/algorithm/geometry/Point.kt
+3 -1
View File
@@ -1,4 +1,6 @@
package algorithm.geometry
class Point(val x: Double, val y: Double) {
data class Point(val x: Double, val y: Double) {
}
server/src/main/java/algorithm/geometry/Util.kt
+1 -9
View File
@@ -2,13 +2,5 @@ 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()
}
val eps = 1e-2
}
server/src/main/java/algorithm/geometry/Wall.kt
+39
View File
@@ -0,0 +1,39 @@
package algorithm.geometry
import java.util.*
data class Wall(val points: ArrayList<Point> = arrayListOf<Point>(),
var line: Line = Line(0.0, 0.0, 0.0)) {
fun addPoint(point: Point) {
points.add(point)
line = approximatePointsByLine()
}
private fun approximatePointsByLine(): Line {
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().x)
}
val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
val b = (sumY - sumX * k) / pointsCount
return Line(-k, 1.0, -b)
}
override fun equals(other: Any?): Boolean {
if (other == null) return false
if (!(other is Wall)) return false
return line.equals(other.line)
}
}