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

make new algorithm. based on car position

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-26 17:55:41 +03:00
parent b6f4a63b63
commit bb1d91ac62
5 changed files with 161 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
server/src/main/java/DebugClInterface.kt
+7 -1
View File
@@ -2,6 +2,7 @@ import Exceptions.InactiveCarException
import algorithm.AbstractAlgorithm
import algorithm.RoomBypassingAlgorithm
import algorithm.RoomModel
import algorithm.RoomTest
import car.client.Client
import io.netty.buffer.Unpooled
import io.netty.handler.codec.http.*
@@ -64,6 +65,10 @@ object DebugClInterface {
val tmp = algorithmImpl
if (tmp is RoomBypassingAlgorithm) {
println("length: ${tmp.wallLength} angleOX: ${tmp.wallAngleWithOX}")
println("x: ${tmp.carX} y: ${tmp.carY} angle:${tmp.carAngle}")
} else if (tmp is RoomTest) {
println("points: ${tmp.points}")
println("x: ${tmp.carX} y: ${tmp.carY} angle:${tmp.carAngle}")
}
}
"alg" -> executeAlg(readString)
@@ -81,7 +86,8 @@ object DebugClInterface {
} else 1
if (algorithmImpl == null) {
algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
algorithmImpl = RoomTest(environment.map.values.last(), exchanger)
// algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
}
while (count > 0) {
count--
server/src/main/java/algorithm/AbstractAlgorithm.kt
+4 -2
View File
@@ -49,7 +49,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
}
try {
val distances = exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
val distances = exchanger.exchange(IntArray(0), 300, TimeUnit.SECONDS)
val result = DoubleArray(angles.size)
for (i in 0..result.size - 1) {
val distancesOnCurrentAngle = distances.drop(i * copyElemCount).take(copyElemCount)
@@ -60,7 +60,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
}
}
if (sum != 0) {
result[i] = (sum.toDouble()) / (copyElemCount*1000000)
result[i] = (sum.toDouble()) / (copyElemCount)
}
}
return result
@@ -113,6 +113,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
return
}
val command = getCommand(anglesDistances, state)
afterGetCommand(command)
println(Arrays.toString(command.directions))
println(Arrays.toString(command.times))
@@ -137,6 +138,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState?
protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest
protected abstract fun afterGetCommand(route:RouteRequest)
private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest {
server/src/main/java/algorithm/RoomBypassingAlgorithm.kt
+26 -2
View File
@@ -16,6 +16,9 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
public var wallLength = 0.0//sm
var errorCount = 0
var carX = 0
var carY = 0
var carAngle = 0
private var prevPoint = Pair(0.0, 0.0)
@@ -40,6 +43,27 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
return args
}
override fun afterGetCommand(route: RouteRequest) {
route.directions.forEachIndexed { idx, direction ->
when (direction) {
FORWARD -> {
carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
}
BACKWARD -> {
carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
}
LEFT -> {
carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt()
}
RIGHT -> {
carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt()
}
}
}
}
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
val dist0 = anglesDistances[0]
val dist60 = anglesDistances[60]
@@ -74,8 +98,8 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
if (dist90 > 40 || dist90 < 20) {
val rotationDirection = if (dist90 > 40) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setTimes(getIntArray((30.0 / ROTATION_VELOCITY).toInt(),
(25.0 / MOVE_VELOCITY).toInt()))
resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(),
(35.0 / MOVE_VELOCITY).toInt()))
return resultBuilder.build()
}
server/src/main/java/algorithm/RoomModel.kt
+28 -17
View File
@@ -1,8 +1,9 @@
package algorithm
import DebugClInterface
import DebugResponse
import Waypoints
import algorithm.geometry.Line
import DebugResponse
import objects.Car
object RoomModel {
@@ -21,27 +22,37 @@ object RoomModel {
var currentPosition_x = 0
var currentPosition_y = 0
fun getUpdate(): Waypoints {
if (iter == 5) {
web.server.Server.changeMode(web.server.Server.ServerMode.IDLE)
return Waypoints.BuilderWaypoints(IntArray(0), IntArray(0), IntArray(0), IntArray(0), true).build()
val algorithm = DebugClInterface.algorithmImpl
if (algorithm == null || !(algorithm is RoomTest)) {
val emptyArr = IntArray(0)
return Waypoints.BuilderWaypoints(emptyArr, emptyArr, emptyArr, emptyArr, false).build()
}
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(10)
val begin_y = IntArray(10)
val end_x = IntArray(10)
val end_y = IntArray(10)
for (i in 0..9) {
begin_x[i] = currentPosition_x
begin_y[i] = currentPosition_y
for (i in 0..algorithm.points.size - 1) {
val curPoint = algorithm.points[i]
if (algorithm.points.size - 1 == i) {
// val nextPoint = algorithm.points[0]
// begin_x[i] = curPoint.first.toInt()
// begin_y[i] = curPoint.second.toInt()
//
// end_x[i] = nextPoint.first.toInt()
// end_y[i] = nextPoint.second.toInt()
continue
}
val nextPoint = algorithm.points[i + 1]
begin_x[i] = curPoint.first.toInt()
begin_y[i] = curPoint.second.toInt()
currentPosition_x += rng.nextInt(3)
currentPosition_y += rng.nextInt(3)
end_x[i] = currentPosition_x
end_y[i] = currentPosition_y
end_x[i] = nextPoint.first.toInt()
end_y[i] = nextPoint.second.toInt()
}
iter++
return Waypoints.BuilderWaypoints(begin_x, begin_y, end_x, end_y, false).build()
}
server/src/main/java/algorithm/RoomTest.kt
+96
View File
@@ -0,0 +1,96 @@
package algorithm
import objects.Car
import java.util.concurrent.Exchanger
import RouteRequest
class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
private val MOVE_VELOCITY = 0.05//sm/ms
private val ROTATION_VELOCITY = 0.05//degrees/ms
val points = arrayListOf<Pair<Double, Double>>()
var carX = 0
var carY = 0
var carAngle = 0
override fun getCarState(anglesDistances: Map<Int, Double>): CarState {
return CarState.WALL
}
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 resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
if (dist90 == null) {
resultBuilder.setDirections(getIntArray(RIGHT))
resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt()))
return resultBuilder.build()
}
val sonarAngle = carAngle - 90
points.add(Pair(carX + dist90 * Math.cos(degreesToRadian(sonarAngle)),
carY + dist90 * Math.sin(degreesToRadian(sonarAngle))))
if (dist120 == null || dist60 == null) {
//hmm
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setTimes(getIntArray((10 / MOVE_VELOCITY).toInt()))
return resultBuilder.build()
}
if (dist0 != null && dist0 < 60) {
resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt()))
return resultBuilder.build()
}
val abs = Math.abs(dist120 - dist60)
if (abs > 10) {
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(), (7 / MOVE_VELOCITY).toInt()))//todo calibrate
return resultBuilder.build()
}
if (dist90 > 50 || dist90 < 25) {
val rotationDirection = if (dist90 > 50) RIGHT else LEFT
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt()))
return resultBuilder.build()
}
resultBuilder.setDirections(getIntArray(FORWARD))
resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt()))
return resultBuilder.build()
}
private fun getIntArray(vararg args: Int): IntArray {
return args
}
override fun afterGetCommand(route: RouteRequest) {
route.directions.forEachIndexed { idx, direction ->
when (direction) {
FORWARD -> {
carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
}
BACKWARD -> {
carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
}
LEFT -> {
carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt()
}
RIGHT -> {
carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt()
}
}
}
}
private fun degreesToRadian(angle: Int): Double {
return Math.PI * angle / 180
}
}