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

working algorithm in large room

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-31 18:19:35 +03:00
parent af1ee06ec6
commit 13ddf0ebf5
5 changed files with 101 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
server/src/main/java/DebugClInterface.kt
+13 -6
View File
@@ -1,5 +1,6 @@
import Exceptions.InactiveCarException
import algorithm.AbstractAlgorithm
import algorithm.AlgorithmThread
import algorithm.RoomBypassingAlgorithm
import algorithm.RoomModel
import car.client.CarClient
@@ -15,6 +16,7 @@ object DebugClInterface {
val exchanger = Exchanger<IntArray>()
var algorithmImpl: AbstractAlgorithm? = null
var algorithmThread: AlgorithmThread? = null
private val routeRegex = Regex("route [0-9]{1,10}")
private val sonarRegex = Regex("sonar [0-9]{1,10}")
@@ -101,20 +103,25 @@ object DebugClInterface {
private fun executeAlg(readString: String) {
val params = readString.split(" ")
var count =
val 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)
var alg = algorithmImpl
if (alg == null) {
alg = RoomBypassingAlgorithm(Environment.map.values.last(), exchanger)
}
while (count > 0) {
count--
algorithmImpl!!.iterate()
var algThread = algorithmThread
if (algThread == null) {
algThread = AlgorithmThread(alg)
algThread.start()
}
algorithmImpl = alg
algThread.setCount(count)
algorithmThread = algThread
}
private fun executeDebugInfoCommand(readString: String) {
server/src/main/java/algorithm/AbstractAlgorithm.kt
+3 -4
View File
@@ -15,6 +15,7 @@ import java.util.*
import java.util.concurrent.Exchanger
import java.util.concurrent.TimeUnit
import java.util.concurrent.TimeoutException
import kotlin.concurrent.thread
abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntArray>) {
@@ -63,8 +64,6 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
try {
val distances = exchanger.exchange(IntArray(0), 300, TimeUnit.SECONDS)
return distances
} catch (e: InterruptedException) {
println("don't have response from car!")
} catch (e: TimeoutException) {
println("don't have response from car. Timeout!")
}
@@ -77,6 +76,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
moveCar(requestBytes)
}
private fun moveCar(messageBytes: ByteArray) {
val request = getDefaultHttpRequest(thisCar.host, setRouteMetricUrl, messageBytes)
try {
@@ -87,8 +88,6 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
try {
exchanger.exchange(IntArray(0), 60, TimeUnit.SECONDS)
return
} catch (e: InterruptedException) {
println("don't have response from car!")
} catch (e: TimeoutException) {
println("don't have response from car. Timeout!")
}
server/src/main/java/algorithm/AlgorithmThread.kt
+31
View File
@@ -0,0 +1,31 @@
package algorithm
import kotlin.concurrent.thread
class AlgorithmThread(val algorithmImpl: AbstractAlgorithm) : Thread() {
private var count = 0
override fun run() {
while (true) {//todo while !alrightmImpl.isDone(), but need check it
try {
if (count > 0) {
algorithmImpl.iterate()
count--
} else {
Thread.sleep(1000)
}
} catch (e: InterruptedException) {
}
}
}
fun setCount(count: Int) {
this.count = count
}
fun cancel() {
count = 0
}
}
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+47 -31
View File
@@ -1,5 +1,6 @@
package algorithm
import Logger.log
import RouteMetricRequest
import SonarRequest
import algorithm.geometry.Angle
@@ -8,7 +9,6 @@ import algorithm.geometry.Point
import algorithm.geometry.Wall
import objects.Car
import java.util.concurrent.Exchanger
import Logger.log
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
@@ -19,14 +19,14 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
override val WINDOW_SIZE = 3
//SHOULD BE CALIBRATED BEFORE RUNNING!!!!!!!!!!!
private val CHARGE_CORRECTION = 0.85
private val CHARGE_CORRECTION = 1.02//on full charge ok is 0.83 - 0.86
private val MAX_DISTANCE_TO_WALL_AHEAD = 30 // reached the corner and should turn left
private val OUTER_CORNER_DISTANCE_THRESHOLD = 60 // reached outer corner
private val MAX_DISTANCE_TO_WALL_AHEAD = 55 // reached the corner and should turn left
private val OUTER_CORNER_DISTANCE_THRESHOLD = 90 // reached outer corner
private val ISOSCALENESS_MIN_DIFF = 5 // have to correct alignment to the wall
private val ISOSCALENESS_MAX_DIFF = 50 // almost reached outer corner but not yet, and 120 meas. founds far wall
private val DISTANCE_TO_WALL_UPPER_BOUND = 50 // have to move closer to the parallel wall
private val DISTANCE_TO_WALL_LOWER_BOUND = 25 // have to move farther to the parallel wall
private val DISTANCE_TO_WALL_UPPER_BOUND = 60 // have to move closer to the parallel wall
private val DISTANCE_TO_WALL_LOWER_BOUND = 40 // have to move farther to the parallel wall
private val SPURIOUS_REFLECTION_DIFF = 70 // we're approaching corner and get spurious reflection from two walls on 60 meas. ! Should be before outer corner case !
private var calibrateAfterRotate = false
@@ -70,7 +70,12 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
for (i in 0..15 step 5) {
val distRightForward = anglesDistances[Angle(70 + i)]!!
val distRightBackward = anglesDistances[Angle(110 - i)]!!
if (distRightBackward.distance > (average * 1.5) || distRightForward.distance > (average * 1.5)) {
if (distRightBackward.distance == -1 || distRightForward.distance == -1) {
log("Found -1 in angle distances, passing")
continue
}
if (distRightBackward.distance > (average * 1.5) || distRightForward.distance > (average * 1.5) ||
distRightBackward.distance < (average / 2) || distRightForward.distance < (average / 2)) {
continue
}
@@ -93,16 +98,20 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
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))
val backRotationDirection = if (rotationDirection == RIGHT) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD, backRotationDirection))
resultBuilder.setDistances(getIntArray(15, 20, 5))
return resultBuilder.build()
}
private fun moveForward(): RouteMetricRequest {
private fun moveForward(distToWall: Int): RouteMetricRequest {
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setDistances(getIntArray(20))
if (distToWall == -1 || distToWall > 200) {
resultBuilder.setDistances(getIntArray(50))
} else {
resultBuilder.setDistances(getIntArray(Math.max(distToWall / 4, 20)))
}
return resultBuilder.build()
}
@@ -126,12 +135,19 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
override fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest? {
val dist0 = anglesDistances[Angle(0)]!!
val dist70 = anglesDistances[Angle(70)]!!
val dist80 = anglesDistances[Angle(80)]!!
val dist90 = anglesDistances[Angle(90)]!!
val dist100 = anglesDistances[Angle(100)]!!
val dist110 = anglesDistances[Angle(110)]!!
val sonarAngle = carAngle - 90
if (anglesDistances.filter { it.value.angle.degs() >= 60 && it.value.angle.degs() <= 120 && it.value.distance == -1 }.size != 0) {
log("Found -1 in angle distances, passing")
if (anglesDistances.filter {
it.value.angle.degs() >= 60
&& it.value.angle.degs() <= 120
&& it.value.distance == -1
}.size > anglesDistances.size / 2) {
log("Found to many -1 in angle distances, passing")
//todo Теоретически такая ситуация может быть валидной, если сразу после внутреннего угла идёт внешний
return null
}
@@ -182,20 +198,20 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
// Big fail too
if (dist110.distance == -1) {
log("No back measurement found, wtf")
return moveForward()
return moveForward(dist0.distance)
}
// Approaching inner corner and getting spurious reflection from 2 walls on 60 - just move forward to get closer to corner
if (dist70.distance - dist0.distance > SPURIOUS_REFLECTION_DIFF) {
log("Spurious reflection detected, moving forward")
return moveForward()
}
// Approaching outer corner (parallel wall is ending soon, but not yet) - just move forward to get to the end of the wall
if (dist70.distance == -1 || Math.abs(dist110.distance - dist70.distance) > ISOSCALENESS_MAX_DIFF) {
log("Aprroaching outer corner, moving forward")
return moveForward()
}
// if (dist70.distance - dist0.distance > SPURIOUS_REFLECTION_DIFF) {
// log("Spurious reflection detected, moving forward")
// return moveForward()
// }
//
// // Approaching outer corner (parallel wall is ending soon, but not yet) - just move forward to get to the end of the wall
// if (dist80.distance == -1 || Math.abs(dist100.distance - dist80.distance) > ISOSCALENESS_MAX_DIFF) {
// log("Aprroaching outer corner, moving forward")
// return moveForward()
// }
for (i in 70..110 step 10) {
if (i == 90) {
@@ -203,7 +219,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
}
val curDist = anglesDistances[Angle(i)]!!.distance
if (curDist > (average * 1.5)) {
if (curDist > (average * 1.5) || curDist < (average / 2)) {
continue
}
val point = Point(
@@ -228,7 +244,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
// default case: everything is ok, just move forward
log("Default case: moving forward")
return moveForward()
return moveForward(dist0.distance)
}
private fun calculateAngle(anglesDistances: Map<Angle, AngleData>, state: CarState): Angle {
@@ -252,12 +268,12 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
carY -= (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
}
LEFT -> {
route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
carAngle += route.distances[idx]
// route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
// carAngle += route.distances[idx]
}
RIGHT -> {
route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
carAngle -= route.distances[idx]
// route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
// carAngle -= route.distances[idx]
}
}
}
server/src/main/java/car/client/ClientHandler.kt
+7
View File
@@ -85,6 +85,13 @@ 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)}")
val algThread = DebugClInterface.algorithmThread
if (algThread == null) {
return
}
if (!algThread.isAlive) {
return
}
try {
DebugClInterface.exchanger.exchange(message.distances, 20, TimeUnit.SECONDS)
} catch (e: InterruptedException) {