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

working algorithm on very simple room

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-25 19:28:05 +03:00
parent aee08025eb
commit 78d51b6ac0
10 changed files with 252 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
car_srv/kotlinSrv/src/Test.kt
+12
View File
@@ -55,6 +55,18 @@ private fun testCarEmulator() {
val dist90 = sonarResponse.distances[0]
eq(dist90.toDouble(), 98.9949, "check angle 90", 2.5)
})
println("--------------------")
CarState.instance.x = 150
CarState.instance.y = 50
CarState.instance.angle = 0
sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(7, { it*5 })).build()
sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build()
controller.executeRequestSensorData(sonarRequest, { bytes ->
sonarResponse.mergeFrom(CodedInputStream(bytes))
val dist30 = sonarResponse.distances[7]
eq(dist30.toDouble(), 98.9949, "check angle 30", 2.5)
})
}
private var testNum = 1
car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt
+16 -12
View File
@@ -11,10 +11,14 @@ import encodeProtoBuf
import geometry.Line
import geometry.Vector
import room.Room
import kotlin.Pair
class ControllerEmulator : Controller {
private val MOVE_VELOCITY = 32.78//sm/s
private val ROTATION_VELOCITY = 12.3//degrees/s
private val MOVE_VELOCITY = 0.05//sm/ms
private val ROTATION_VELOCITY = 0.05//degrees/ms
private val ADD_RANDOM = false
enum class MoveDirection {
LEFT,
@@ -87,7 +91,7 @@ class ControllerEmulator : Controller {
if (distance == -1) {
distances.add(distance)
} else {
val delta = getRandomIntFrom(IntArray(5, { x -> x - 2 }))//return one of -2 -1 0 1 or 2
val delta = if (ADD_RANDOM) getRandomIntFrom(IntArray(5, { x -> x - 2 })) else 0//return one of -2 -1 0 1 or 2
distances.add(distance + delta)
}
}
@@ -119,14 +123,14 @@ class ControllerEmulator : Controller {
continue
}
val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0)
+ Math.pow(yIntersection - ySensor0, 2.0)))
+ Math.pow(yIntersection - ySensor0, 2.0))*100000)
if (currentDistance < result) {
result = currentDistance
}
}
if (result < 5 || result > 500) {
return -1
}
// if (result < 5 || result > 500) {
// return -1
// }
return result
}
@@ -158,13 +162,13 @@ class ControllerEmulator : Controller {
//refresh car state
val carInstance = CarState.instance
val commandTime = currentCommand.second
val delta = Math.random() * 0.2 + 0.9// delta in [0.9, 1.1)
val delta = if (ADD_RANDOM) Math.random() * 0.2 + 0.9 else 1.0// delta in [0.9, 1.1)
val commandTimeIncludeRandom = (commandTime * delta).toInt()
when (currentCommand.first) {
MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt() / 1000)
MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt() / 1000)
MoveDirection.RIGHT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt() / 1000)
MoveDirection.LEFT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt() / 1000)
MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt())
MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt())
MoveDirection.RIGHT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt())
MoveDirection.LEFT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt())
else -> {
}
}
car_srv/kotlinSrv/src/room/Room.kt
+29 -32
View File
@@ -32,45 +32,42 @@ object Room {
|___|
*/
val upLine = Line(0.0, 1.0, -300.0)
val leftLine = Line(1.0, 0.0, 150.0)
val bottomLine = Line(0.0, 1.0, 20.0)
val rightLine = Line(1.0, 0.0, -200.0)
val bottomLine2 = Line(0.0, 1.0, -100.0)
val rightLine2 = Line(1.0, 0.0, -300.0)
val walls = listOf<Wall>(Wall(upLine, 300, -150, 300, 300),
Wall(leftLine, -150, -150, 300, -20),
Wall(bottomLine, -150, 200, -20, -20),
Wall(rightLine, 200, 200, -20, 100),
Wall(bottomLine2, 200, 300, 100, 100),
Wall(rightLine2, 300, 300, 100, 300)
)
// val upLine = Line(0.0, 1.0, -300.0)
// val leftLine = Line(1.0, 0.0, 150.0)
// val bottomLine = Line(0.0, 1.0, 20.0)
// val rightLine = Line(1.0, 0.0, -200.0)
//
// val bottomLine2 = Line(0.0, 1.0, -100.0)
// val rightLine2 = Line(1.0, 0.0, -300.0)
//
// val walls = listOf<Wall>(Wall(upLine, 300, -150, 300, 300),
// Wall(leftLine, -150, -150, 300, -20),
// Wall(bottomLine, -150, 200, -20, -20),
// Wall(rightLine, 200, 200, -20, 100),
// Wall(bottomLine2, 200, 300, 100, 100),
// Wall(rightLine2, 300, 300, 100, 300)
// )
/*
________
| __|
_______
| |
|____/
*/
// val upLine = Line(0.0, 1.0, -300.0)
// val leftLine = Line(1.0, 0.0, 150.0)
// val bottomLine = Line(0.0, 1.0, 20.0)
// val rightLine = Line(-1.2, 1.0, 140.0)
//
// val bottomLine2 = Line(0.0, 1.0, -100.0)
// val rightLine2 = Line(1.0, 1.0, -350.0)
//
// val walls = listOf<Wall>(Wall(upLine, 350, -150, 300, 300),
// Wall(leftLine, -150, -150, 300, -20),
// Wall(bottomLine, -150, 100, -20, -20),
// Wall(rightLine, 100, 200, -20, 100),
// Wall(bottomLine2, 200, 350, 100, 100),
// Wall(rightLine2, 350, 350, 100, 300)
// )
val upLine = Line(0.0, 1.0, -300.0)
val leftLine = Line(1.0, 0.0, 150.0)
val bottomLine = Line(0.0, 1.0, 20.0)
val rightLine = Line(-1.2, 1.0, 140.0)
val rightLine2 = Line(1.0, 1.0, -350.0)
val walls = listOf<Wall>(Wall(upLine, 200, -150, 300, 300),
Wall(leftLine, -150, -150, 300, -20),
Wall(bottomLine, -150, 100, -20, -20),
Wall(rightLine, 100, 200, -20, 100),
Wall(rightLine2, 200, 200, 100, 300)
)
}
server/src/main/java/DebugClInterface.kt
+26 -5
View File
@@ -1,6 +1,7 @@
import Exceptions.InactiveCarException
import algorithm.AbstractAlgorithm
import algorithm.RoomBypassingAlgorithm
import algorithm.RoomModel
import car.client.Client
import io.netty.buffer.Unpooled
import io.netty.handler.codec.http.*
@@ -10,7 +11,7 @@ import java.util.concurrent.Exchanger
object DebugClInterface {
val exchanger = Exchanger<IntArray>()
var algorithm: AbstractAlgorithm? = null
var algorithmImpl: AbstractAlgorithm? = null
private val routeRegex = Regex("route [0-9]{1,10}")
private val sonarRegex = Regex("sonar [0-9]{1,10}")
@@ -56,16 +57,36 @@ object DebugClInterface {
"refloc" -> executeRefreshLocationCommand()
"sonar" -> executeSonarCommand(readString)
"dbinfo" -> executeDebugInfoCommand(readString)
"alg" -> {
if (algorithm == null) {
algorithm = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
"points" -> println(algorithm.RoomModel.lines)
"pos" -> {
val tmp = algorithmImpl
if (tmp is RoomBypassingAlgorithm) {
println("length: ${tmp.wallLength} angleOX: ${tmp.wallAngleWithOX}")
}
algorithm!!.iterate()
}
"alg" -> executeAlg(readString)
else -> printNotSupportedCommand(readString)
}
}
private fun executeAlg(readString: String) {
val params = readString.split(" ")
var count =
if (params.size == 2) try {
params[1].toInt()
} catch (e: Exception) {
1
} else 1
if (algorithmImpl == null) {
algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
}
while (count > 0) {
count--
algorithmImpl!!.iterate()
}
}
private fun executeDebugInfoCommand(readString: String) {
val params = readString.split(" ")
if (params.size != 3) {
server/src/main/java/algorithm/AbstractAlgorithm.kt
+7 -5
View File
@@ -21,7 +21,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
protected val LEFT = 2
protected val RIGHT = 3
private var prevState = CarState.WALL
private var prevState:CarState? = null
private var prevSonarDistances = mapOf<Int, Double>()
@@ -33,7 +33,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
protected fun getData(angles: IntArray): DoubleArray {
val copyElemCount = 5
val copyElemCount = 1
val anglesCoped = IntArray(angles.size * copyElemCount, { angles[it / copyElemCount] })
val message = SonarRequest.BuilderSonarRequest(anglesCoped).build()
@@ -50,7 +50,6 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
val distances = exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
val result = DoubleArray(angles.size)
for (i in 0..result.size - 1) {
//todo sonar can send -1!!!
val distancesOnCurrentAngle = distances.drop(i * copyElemCount).take(copyElemCount)
var sum = 0
for (distance in distancesOnCurrentAngle) {
@@ -59,7 +58,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
}
}
if (sum != 0) {
result[i] = sum.toDouble() / copyElemCount
result[i] = (sum.toDouble()) / (100000*copyElemCount)
}
}
return result
@@ -108,6 +107,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
val state = getCarState(anglesDistances)
val command = getCommand(anglesDistances, state)
calculateCarPosition(command, state)
println(Arrays.toString(command.directions))
println(Arrays.toString(command.times))
@@ -117,7 +117,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
moveCar(command)
}
protected fun getPrevState(): CarState {
protected fun getPrevState(): CarState? {
return prevState
}
@@ -125,6 +126,7 @@ 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
protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest
protected abstract fun getAngles(): IntArray
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+133 -77
View File
@@ -1,36 +1,37 @@
package algorithm
import RouteRequest
import algorithm.geometry.Line
import algorithm.geometry.Vector
import objects.Car
import java.util.*
import java.util.concurrent.Exchanger
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
private val MOVE_VELOCITY = 30.3//sm/s
private val ROTATION_VELOCITY = 11.0//degrees/s
private val MOVE_VELOCITY = 0.05//sm/ms
private val ROTATION_VELOCITY = 0.05//degrees/ms
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
private var wallLength = 0.0//in sm
private var wallAngle = 0.0//in radian
private var xPos = 0.0
private var yPos = 0.0
private var prevPoint = Pair(0.0, 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) {
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,...
return CarState.OUTER
}
if (dist90 < 20) {
return CarState.WALL
}
if (dist0 > 60) {
if (dist0 == null || dist0 > 70) {
return CarState.WALL
}
return CarState.INNER
@@ -41,125 +42,180 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
}
override fun getAngles(): IntArray {
return IntArray(19, { it * 10 })
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]!!
val dist90 = anglesDistances[90]!!
val dist120 = anglesDistances[120]!!
val dist0 = anglesDistances[0]
val dist60 = anglesDistances[60]
val dist90 = anglesDistances[90]
val dist120 = anglesDistances[120]
val dist180 = anglesDistances[180]
val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
if (dist120 == null || dist90 == null || dist60 == null) {
println("null distance!")
return resultBuilder.build()
}
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
}
}
when (state) {
CarState.WALL -> {
if (wallLength.toInt() == 0 && (dist0 != null && dist180 != null)) {
wallLength = dist0 + dist180
}
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))
resultBuilder.setTimes(getIntArray((30.0 / ROTATION_VELOCITY).toInt(),
(25.0 / MOVE_VELOCITY).toInt()))
return resultBuilder.build()
}
if (Math.abs(dist120 - dist60) > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection))
resultBuilder.setTimes(getIntArray(1000))
resultBuilder.setTimes(getIntArray((15.0 / ROTATION_VELOCITY).toInt()))
return resultBuilder.build()
}
resultBuilder.setDirections(getIntArray(FORWARD))
wallLength += 1 * MOVE_VELOCITY
resultBuilder.setTimes(getIntArray(1000))
resultBuilder.setTimes(getIntArray((25.0 / MOVE_VELOCITY).toInt()))
return resultBuilder.build()
}
CarState.INNER -> {
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)
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
//в угле первое действие - встать максимально паралельно стене.
if (Math.abs(dist120 - dist60) > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection))
resultBuilder.setTimes(getIntArray((3.0 / ROTATION_VELOCITY).toInt()))
return resultBuilder.build()
}
//если стоим паралельно - можно мерить угол
val pointsNextLine = arrayListOf<Pair<Double, Double>>()
for (angleDist in anglesDistances) {
if (angleDist.key <= 15) {
pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair()))
}
}
val nextLine = approximatePointsByLine(pointsNextLine.toTypedArray())
val currentLine = RoomModel.lines.last()
val xIntersection = prevPoint.first + wallLength * Math.cos(wallAngleWithOX)
val yIntersection = prevPoint.second + wallLength * Math.sin(wallAngleWithOX)
nextLine.C = -nextLine.A * xIntersection - nextLine.B * yIntersection
var xOnNextLine = xIntersection + 1
var yOnNextLine = (-nextLine.C - nextLine.A * xOnNextLine) / nextLine.B
//точка должна находится слева от нашей правой стены
if (currentLine.A * xOnNextLine + currentLine.B * yOnNextLine + currentLine.C < 0) {
xOnNextLine = xIntersection - 1
yOnNextLine = (-nextLine.C - nextLine.A * xOnNextLine) / nextLine.B
}
val vectorNextLine = Vector(xIntersection, yIntersection, xOnNextLine, yOnNextLine)
val vectorCurrentLine = Vector(xIntersection, yIntersection, prevPoint.first, prevPoint.second)
val scalarProduct = vectorCurrentLine.scalarProduct(vectorNextLine)
val angle = Math.acos(scalarProduct / (vectorCurrentLine.length() * vectorNextLine.length()))
val wallsAngleInDegrees = radiansToDegrees(angle)
wallAngleWithOX += degreesToRadian(180 - wallsAngleInDegrees.toInt())
prevPoint = Pair(xIntersection, yIntersection)
wallLength = 0.0
RoomModel.lines.add(nextLine)
resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT))
resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt()))
resultBuilder.setTimes(getIntArray((radiansToDegrees(wallAngleWithOX) / (2 * ROTATION_VELOCITY)).toInt(),
(40 / MOVE_VELOCITY).toInt(), (radiansToDegrees(wallAngleWithOX) / (2 * ROTATION_VELOCITY)).toInt()))
return resultBuilder.build()
}
CarState.OUTER -> {
//todo calculate angle
resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT))
resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt()))
resultBuilder.setTimes(getIntArray((45 / ROTATION_VELOCITY).toInt(),
(40 / MOVE_VELOCITY).toInt(), (45 / 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 radiansToDegrees(angle: Double): Double {
return angle * 180 / Math.PI
}
private fun sonarAngleDistToPoint(angleDist: Pair<Int, Double>): Pair<Double, Double> {
val angle = wallAngle - degreesToRadian(angleDist.first)
val angle = wallAngleWithOX - degreesToRadian(angleDist.first)
val dist = angleDist.second
val result = Pair(xPos + Math.cos(angle) * dist, yPos + Math.sin(angle) * dist)
val result = Pair(Math.cos(angle) * dist, 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> {
private fun approximatePointsByLine(points: Array<Pair<Double, Double>>): Line {
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 p1 = points.first()
val p2 = points[1]
val pointsCount = points.size
val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
val b = (sumY - sumX * k) / pointsCount
return Line(p2.second - p1.second, p1.first - p2.first, -p1.first * p2.second + p1.second * p2.first)
return Pair(k, b)
//
// 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 Line(-k, 1.0, -b)
}
private fun calculateAngleWithWall(anglesDistances: MutableMap<Int, Double>): Double {
val dist60 = anglesDistances[60]!!
val dist120 = anglesDistances[120]!!
val dist60 = anglesDistances[60]
val dist120 = anglesDistances[120]
//Math.cos(60) = 1/2
val wallLength = Math.sqrt(Math.pow(dist60, 2.0) + Math.pow(dist120, 2.0) - dist120 * dist60)//in triangle
val hOnWall = getRangeToWall(wallLength, dist60, dist120)
// val wallLength = Math.sqrt(Math.pow(dist60, 2.0) + Math.pow(dist120, 2.0) - dist120 * dist60)//in triangle
//
// val hOnWall = getRangeToWall(wallLength, dist60, dist120)
return 0.0
}
server/src/main/java/algorithm/RoomModel.kt
+4 -4
View File
@@ -1,11 +1,11 @@
package algorithm
import Waypoints
/**
* Created by user on 8/23/16.
*/
import algorithm.geometry.Line
object RoomModel {
val point = arrayListOf<Pair<Int, Int>>()
val point = arrayListOf<Pair<Double, Double>>()
val lines = arrayListOf<Line>()
// DEBUG BELOW
server/src/main/java/algorithm/geometry/Line.kt
+21
View File
@@ -0,0 +1,21 @@
package algorithm.geometry
class Line(val A: Double, val B: Double, var C: Double) {
fun getIntersectionPoint(lineTwo: Line): Pair<Double, Double> {
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)
}
override fun toString(): String{
return "Line(A=$A, B=$B, C=$C)"
}
}
server/src/main/java/algorithm/Vector.kt → server/src/main/java/algorithm/geometry/Vector.kt
+1 -1
View File
@@ -1,4 +1,4 @@
package algorithm
package algorithm.geometry
class Vector constructor(val x: Double, val y: Double) {
server/src/main/java/car/client/ClientHandler.kt
+3 -2
View File
@@ -14,6 +14,7 @@ import io.netty.util.AttributeKey
import objects.Environment
import setRouteUrl
import sonarUrl
import java.util.*
import java.util.concurrent.TimeUnit
import java.util.concurrent.TimeoutException
@@ -84,8 +85,8 @@ class ClientHandler : SimpleChannelInboundHandler<Any> {
}
private fun handlerSonarResponse(message: SonarResponse, angles: IntArray) {
// println("request angles: ${Arrays.toString(angles)}")
// println("distances from sonar: ${Arrays.toString(message.distances)}")
println("request angles: ${Arrays.toString(angles)}")
println("distances from sonar: ${Arrays.toString(message.distances)}")
try {
DebugClInterface.exchanger.exchange(message.distances, 20, TimeUnit.SECONDS)
} catch (e: InterruptedException) {