add work with metric route
This commit is contained in:
@@ -18,7 +18,7 @@ class ControllerEmulator : Controller {
|
|||||||
private val MOVE_VELOCITY = 0.05//sm/ms
|
private val MOVE_VELOCITY = 0.05//sm/ms
|
||||||
private val ROTATION_VELOCITY = 0.05//degrees/ms
|
private val ROTATION_VELOCITY = 0.05//degrees/ms
|
||||||
|
|
||||||
private val ADD_RANDOM = true
|
private val ADD_RANDOM = false
|
||||||
|
|
||||||
enum class MoveDirection {
|
enum class MoveDirection {
|
||||||
LEFT,
|
LEFT,
|
||||||
@@ -32,7 +32,7 @@ class ControllerEmulator : Controller {
|
|||||||
val moveTimes = route.times
|
val moveTimes = route.times
|
||||||
val moveDirections = route.directions
|
val moveDirections = route.directions
|
||||||
//list of move direction and time to this move in ms
|
//list of move direction and time to this move in ms
|
||||||
val commands: MutableList<Pair<MoveDirection, Int>> = mutableListOf()
|
val commands: MutableList<Pair<MoveDirection, Double>> = mutableListOf()
|
||||||
|
|
||||||
moveTimes.forEachIndexed { idx, value ->
|
moveTimes.forEachIndexed { idx, value ->
|
||||||
val moveDirection =
|
val moveDirection =
|
||||||
@@ -44,13 +44,31 @@ class ControllerEmulator : Controller {
|
|||||||
else -> MoveDirection.ERROR
|
else -> MoveDirection.ERROR
|
||||||
}
|
}
|
||||||
|
|
||||||
commands.add(Pair(moveDirection, value))
|
if (moveDirection == MoveDirection.FORWARD || moveDirection == MoveDirection.BACKWARD) {
|
||||||
|
commands.add(Pair(moveDirection, value * MOVE_VELOCITY))
|
||||||
|
} else {
|
||||||
|
commands.add(Pair(moveDirection, value * ROTATION_VELOCITY))
|
||||||
|
}
|
||||||
}
|
}
|
||||||
executeCommand(commands, 0, callback)
|
executeCommand(commands, 0, callback)
|
||||||
}
|
}
|
||||||
|
|
||||||
override fun executeMetricRoute(request: RouteMetricRequest, callback: (ByteArray) -> Unit) {
|
override fun executeMetricRoute(request: RouteMetricRequest, callback: (ByteArray) -> Unit) {
|
||||||
throw UnsupportedOperationException()
|
val commands: MutableList<Pair<MoveDirection, Double>> = mutableListOf()
|
||||||
|
|
||||||
|
val moveDistances = request.distances
|
||||||
|
moveDistances.forEachIndexed { idx, value ->
|
||||||
|
val moveDirection =
|
||||||
|
when (request.directions[idx]) {
|
||||||
|
0 -> MoveDirection.FORWARD
|
||||||
|
1 -> MoveDirection.BACKWARD
|
||||||
|
2 -> MoveDirection.LEFT
|
||||||
|
3 -> MoveDirection.RIGHT
|
||||||
|
else -> MoveDirection.ERROR
|
||||||
|
}
|
||||||
|
commands.add(Pair(moveDirection, value.toDouble()))
|
||||||
|
}
|
||||||
|
executeCommand(commands, 0, callback)
|
||||||
}
|
}
|
||||||
|
|
||||||
override fun executeRequestSensorData(sonarRequest: SonarRequest, callback: (ByteArray) -> Unit) {
|
override fun executeRequestSensorData(sonarRequest: SonarRequest, callback: (ByteArray) -> Unit) {
|
||||||
@@ -151,7 +169,7 @@ class ControllerEmulator : Controller {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
fun executeCommand(commands: List<Pair<MoveDirection, Int>>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) {
|
fun executeCommand(commands: List<Pair<MoveDirection, Double>>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) {
|
||||||
if (currentCommandIdx == commands.size) {
|
if (currentCommandIdx == commands.size) {
|
||||||
val responseMessage = RouteResponse.BuilderRouteResponse(0).build()
|
val responseMessage = RouteResponse.BuilderRouteResponse(0).build()
|
||||||
callBack.invoke(encodeProtoBuf(responseMessage))
|
callBack.invoke(encodeProtoBuf(responseMessage))
|
||||||
@@ -161,14 +179,14 @@ class ControllerEmulator : Controller {
|
|||||||
|
|
||||||
//refresh car state
|
//refresh car state
|
||||||
val carInstance = CarState.instance
|
val carInstance = CarState.instance
|
||||||
val commandTime = currentCommand.second
|
val commandDistance = currentCommand.second
|
||||||
val delta = if (ADD_RANDOM) Math.random() * 0.2 + 0.9 else 1.0// 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()
|
val commandDistanceIncludeRandom = (commandDistance * delta).toInt()
|
||||||
when (currentCommand.first) {
|
when (currentCommand.first) {
|
||||||
MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt())
|
MoveDirection.FORWARD -> carInstance.moving(commandDistanceIncludeRandom)
|
||||||
MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt())
|
MoveDirection.BACKWARD -> carInstance.moving(commandDistanceIncludeRandom)
|
||||||
MoveDirection.RIGHT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt())
|
MoveDirection.RIGHT -> carInstance.rotate(-commandDistanceIncludeRandom)
|
||||||
MoveDirection.LEFT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt())
|
MoveDirection.LEFT -> carInstance.rotate(commandDistanceIncludeRandom)
|
||||||
else -> {
|
else -> {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -5,6 +5,7 @@ val carServerPort: Int = 7925
|
|||||||
val webServerPort: Int = 7926
|
val webServerPort: Int = 7926
|
||||||
val getLocationUrl = "/getLocation"
|
val getLocationUrl = "/getLocation"
|
||||||
val setRouteUrl = "/route"
|
val setRouteUrl = "/route"
|
||||||
|
val setRouteMetricUrl = "/routeMetric"
|
||||||
val connectUrl = "/connect"
|
val connectUrl = "/connect"
|
||||||
val debugMemoryUrl = "/debug/memory"
|
val debugMemoryUrl = "/debug/memory"
|
||||||
val sonarUrl = "/sonar"
|
val sonarUrl = "/sonar"
|
||||||
|
|||||||
@@ -2,11 +2,12 @@ package algorithm
|
|||||||
|
|
||||||
import CodedOutputStream
|
import CodedOutputStream
|
||||||
import Exceptions.InactiveCarException
|
import Exceptions.InactiveCarException
|
||||||
import RouteRequest
|
import RouteMetricRequest
|
||||||
import SonarRequest
|
import SonarRequest
|
||||||
import io.netty.buffer.Unpooled
|
import io.netty.buffer.Unpooled
|
||||||
import io.netty.handler.codec.http.*
|
import io.netty.handler.codec.http.*
|
||||||
import objects.Car
|
import objects.Car
|
||||||
|
import setRouteMetricUrl
|
||||||
import setRouteUrl
|
import setRouteUrl
|
||||||
import sonarUrl
|
import sonarUrl
|
||||||
import java.util.*
|
import java.util.*
|
||||||
@@ -72,17 +73,21 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
|
|||||||
return DoubleArray(0)
|
return DoubleArray(0)
|
||||||
}
|
}
|
||||||
|
|
||||||
protected fun moveCar(message: RouteRequest) {
|
protected fun moveCar(message: RouteMetricRequest) {
|
||||||
val requestBytes = ByteArray(message.getSizeNoTag())
|
val requestBytes = ByteArray(message.getSizeNoTag())
|
||||||
message.writeTo(CodedOutputStream(requestBytes))
|
message.writeTo(CodedOutputStream(requestBytes))
|
||||||
val request = getDefaultHttpRequest(thisCar.host, setRouteUrl, requestBytes)
|
moveCar(requestBytes)
|
||||||
|
}
|
||||||
|
|
||||||
|
private fun moveCar(messageBytes: ByteArray) {
|
||||||
|
val request = getDefaultHttpRequest(thisCar.host, setRouteMetricUrl, messageBytes)
|
||||||
try {
|
try {
|
||||||
car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf<String, Int>())
|
car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf<String, Int>())
|
||||||
} catch (e: InactiveCarException) {
|
} catch (e: InactiveCarException) {
|
||||||
println("connection error!")
|
println("connection error!")
|
||||||
}
|
}
|
||||||
try {
|
try {
|
||||||
exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
|
exchanger.exchange(IntArray(0), 60, TimeUnit.SECONDS)
|
||||||
return
|
return
|
||||||
} catch (e: InterruptedException) {
|
} catch (e: InterruptedException) {
|
||||||
println("don't have response from car!")
|
println("don't have response from car!")
|
||||||
@@ -115,7 +120,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
|
|||||||
val command = getCommand(anglesDistances, state)
|
val command = getCommand(anglesDistances, state)
|
||||||
afterGetCommand(command)
|
afterGetCommand(command)
|
||||||
println(Arrays.toString(command.directions))
|
println(Arrays.toString(command.directions))
|
||||||
println(Arrays.toString(command.times))
|
println(Arrays.toString(command.distances))
|
||||||
|
|
||||||
this.prevSonarDistances = anglesDistances
|
this.prevSonarDistances = anglesDistances
|
||||||
this.prevState = state
|
this.prevState = state
|
||||||
@@ -137,8 +142,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
|
|||||||
}
|
}
|
||||||
|
|
||||||
protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState?
|
protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState?
|
||||||
protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest
|
protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest
|
||||||
protected abstract fun afterGetCommand(route:RouteRequest)
|
protected abstract fun afterGetCommand(route: RouteMetricRequest)
|
||||||
|
|
||||||
|
|
||||||
private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest {
|
private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest {
|
||||||
|
|||||||
@@ -1,16 +1,13 @@
|
|||||||
package algorithm
|
package algorithm
|
||||||
|
|
||||||
import RouteRequest
|
|
||||||
import algorithm.geometry.Line
|
import algorithm.geometry.Line
|
||||||
import algorithm.geometry.Vector
|
import algorithm.geometry.Vector
|
||||||
import objects.Car
|
import objects.Car
|
||||||
|
import RouteMetricRequest
|
||||||
import java.util.concurrent.Exchanger
|
import java.util.concurrent.Exchanger
|
||||||
|
|
||||||
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
|
class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
|
||||||
|
|
||||||
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 wallAngleWithOX = 0.0//in radian
|
||||||
public var wallLength = 0.0//sm
|
public var wallLength = 0.0//sm
|
||||||
var errorCount = 0
|
var errorCount = 0
|
||||||
@@ -41,40 +38,40 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
|
|||||||
return args
|
return args
|
||||||
}
|
}
|
||||||
|
|
||||||
override fun afterGetCommand(route: RouteRequest) {
|
override fun afterGetCommand(route: RouteMetricRequest) {
|
||||||
route.directions.forEachIndexed { idx, direction ->
|
route.directions.forEachIndexed { idx, direction ->
|
||||||
when (direction) {
|
when (direction) {
|
||||||
FORWARD -> {
|
FORWARD -> {
|
||||||
carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
|
carX += (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
|
||||||
carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
|
carY += (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
|
||||||
}
|
}
|
||||||
BACKWARD -> {
|
BACKWARD -> {
|
||||||
carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
|
carX -= (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
|
||||||
carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
|
carY -= (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
|
||||||
}
|
}
|
||||||
LEFT -> {
|
LEFT -> {
|
||||||
carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt()
|
carAngle += route.distances[idx]
|
||||||
}
|
}
|
||||||
RIGHT -> {
|
RIGHT -> {
|
||||||
carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt()
|
carAngle -= route.distances[idx]
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
|
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest {
|
||||||
val dist0 = anglesDistances[0]
|
val dist0 = anglesDistances[0]
|
||||||
val dist60 = anglesDistances[60]
|
val dist60 = anglesDistances[60]
|
||||||
val dist90 = anglesDistances[90]
|
val dist90 = anglesDistances[90]
|
||||||
val dist120 = anglesDistances[120]
|
val dist120 = anglesDistances[120]
|
||||||
val dist180 = anglesDistances[180]
|
val dist180 = anglesDistances[180]
|
||||||
val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
|
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
|
||||||
if (dist120 == null || dist90 == null || dist60 == null) {
|
if (dist120 == null || dist90 == null || dist60 == null) {
|
||||||
println("null distance!")
|
println("null distance!")
|
||||||
if (errorCount >= 3) {
|
if (errorCount >= 3) {
|
||||||
errorCount = 0
|
errorCount = 0
|
||||||
resultBuilder.setDirections(getIntArray(BACKWARD))
|
resultBuilder.setDirections(getIntArray(BACKWARD))
|
||||||
resultBuilder.setTimes(getIntArray((15.0 / MOVE_VELOCITY).toInt()))
|
resultBuilder.setDistances(getIntArray(15))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
errorCount++
|
errorCount++
|
||||||
@@ -96,20 +93,19 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
|
|||||||
if (dist90 > 40 || dist90 < 20) {
|
if (dist90 > 40 || dist90 < 20) {
|
||||||
val rotationDirection = if (dist90 > 40) RIGHT else LEFT
|
val rotationDirection = if (dist90 > 40) RIGHT else LEFT
|
||||||
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
|
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
|
||||||
resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(),
|
resultBuilder.setDistances(getIntArray(10, 35))
|
||||||
(35.0 / MOVE_VELOCITY).toInt()))
|
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
|
|
||||||
if (Math.abs(dist120 - dist60) > 10) {
|
if (Math.abs(dist120 - dist60) > 10) {
|
||||||
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
|
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
|
||||||
resultBuilder.setDirections(getIntArray(rotationDirection))
|
resultBuilder.setDirections(getIntArray(rotationDirection))
|
||||||
resultBuilder.setTimes(getIntArray((15.0 / ROTATION_VELOCITY).toInt()))
|
resultBuilder.setDistances(getIntArray(15))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
|
|
||||||
resultBuilder.setDirections(getIntArray(FORWARD))
|
resultBuilder.setDirections(getIntArray(FORWARD))
|
||||||
resultBuilder.setTimes(getIntArray((35.0 / MOVE_VELOCITY).toInt()))
|
resultBuilder.setDistances(getIntArray(35))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
CarState.INNER -> {
|
CarState.INNER -> {
|
||||||
@@ -118,7 +114,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
|
|||||||
if (Math.abs(dist120 - dist60) > 10) {
|
if (Math.abs(dist120 - dist60) > 10) {
|
||||||
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
|
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
|
||||||
resultBuilder.setDirections(getIntArray(rotationDirection))
|
resultBuilder.setDirections(getIntArray(rotationDirection))
|
||||||
resultBuilder.setTimes(getIntArray((4 * 3.0 / ROTATION_VELOCITY).toInt()))//todo calibrate
|
resultBuilder.setDistances(getIntArray(12))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -162,15 +158,13 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
|
|||||||
wallLength = 0.0
|
wallLength = 0.0
|
||||||
RoomModel.lines.add(nextLine)
|
RoomModel.lines.add(nextLine)
|
||||||
resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT))
|
resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT))
|
||||||
resultBuilder.setTimes(getIntArray((wallsAngleInDegrees / (2 * ROTATION_VELOCITY)).toInt(),
|
resultBuilder.setDistances(getIntArray((angle / 2).toInt(), 15, (angle / 2).toInt()))
|
||||||
(40 / MOVE_VELOCITY).toInt(), (wallsAngleInDegrees / (2 * ROTATION_VELOCITY)).toInt()))
|
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
CarState.OUTER -> {
|
CarState.OUTER -> {
|
||||||
//todo calculate target
|
//todo calculate target
|
||||||
resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT))
|
resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT))
|
||||||
resultBuilder.setTimes(getIntArray((45 / ROTATION_VELOCITY).toInt(),
|
resultBuilder.setDistances(getIntArray(45, 40, 45))
|
||||||
(40 / MOVE_VELOCITY).toInt(), (45 / ROTATION_VELOCITY).toInt()))
|
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -2,16 +2,12 @@ package algorithm
|
|||||||
|
|
||||||
import objects.Car
|
import objects.Car
|
||||||
import java.util.concurrent.Exchanger
|
import java.util.concurrent.Exchanger
|
||||||
import RouteRequest
|
import RouteMetricRequest
|
||||||
|
|
||||||
class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
|
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>>()
|
val points = arrayListOf<Pair<Double, Double>>()
|
||||||
|
|
||||||
|
|
||||||
var carX = 0
|
var carX = 0
|
||||||
var carY = 0
|
var carY = 0
|
||||||
var carAngle = 0
|
var carAngle = 0
|
||||||
@@ -20,47 +16,46 @@ class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm
|
|||||||
return CarState.WALL
|
return CarState.WALL
|
||||||
}
|
}
|
||||||
|
|
||||||
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
|
override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteMetricRequest {
|
||||||
val dist0 = anglesDistances[0]
|
val dist0 = anglesDistances[0]
|
||||||
val dist60 = anglesDistances[60]
|
val dist60 = anglesDistances[60]
|
||||||
val dist90 = anglesDistances[90]
|
val dist90 = anglesDistances[90]
|
||||||
val dist120 = anglesDistances[120]
|
val dist120 = anglesDistances[120]
|
||||||
val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
|
val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
|
||||||
if (dist90 == null) {
|
if (dist90 == null) {
|
||||||
resultBuilder.setDirections(getIntArray(RIGHT))
|
resultBuilder.setDirections(getIntArray(RIGHT))
|
||||||
resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt()))
|
resultBuilder.setDistances(getIntArray(10))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
val sonarAngle = carAngle - 90
|
val sonarAngle = carAngle - 90
|
||||||
points.add(Pair(carX + dist90 * Math.cos(degreesToRadian(sonarAngle)),
|
points.add(Pair(carX + dist90 * Math.cos(degreesToRadian(sonarAngle)),
|
||||||
carY + dist90 * Math.sin(degreesToRadian(sonarAngle))))
|
carY + dist90 * Math.sin(degreesToRadian(sonarAngle))))
|
||||||
if (dist120 == null || dist60 == null) {
|
if (dist120 == null || dist60 == null) {
|
||||||
//hmm
|
|
||||||
resultBuilder.setDirections(getIntArray(FORWARD))
|
resultBuilder.setDirections(getIntArray(FORWARD))
|
||||||
resultBuilder.setTimes(getIntArray((10 / MOVE_VELOCITY).toInt()))
|
resultBuilder.setDistances(getIntArray(10))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
if (dist0 != null && dist0 < 60) {
|
if (dist0 != null && dist0 < 60) {
|
||||||
resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
|
resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
|
||||||
resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt()))
|
resultBuilder.setDistances(getIntArray(20, 10))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
val abs = Math.abs(dist120 - dist60)
|
val abs = Math.abs(dist120 - dist60)
|
||||||
if (abs > 10) {
|
if (abs > 10) {
|
||||||
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
|
val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
|
||||||
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
|
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
|
||||||
resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(), (7 / MOVE_VELOCITY).toInt()))//todo calibrate
|
resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
if (dist90 > 50 || dist90 < 25) {
|
if (dist90 > 50 || dist90 < 25) {
|
||||||
val rotationDirection = if (dist90 > 50) RIGHT else LEFT
|
val rotationDirection = if (dist90 > 50) RIGHT else LEFT
|
||||||
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
|
resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
|
||||||
resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt()))
|
resultBuilder.setDistances(getIntArray(10, 10))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
|
|
||||||
resultBuilder.setDirections(getIntArray(FORWARD))
|
resultBuilder.setDirections(getIntArray(FORWARD))
|
||||||
resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt()))
|
resultBuilder.setDistances(getIntArray(20))
|
||||||
return resultBuilder.build()
|
return resultBuilder.build()
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -68,22 +63,22 @@ class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm
|
|||||||
return args
|
return args
|
||||||
}
|
}
|
||||||
|
|
||||||
override fun afterGetCommand(route: RouteRequest) {
|
override fun afterGetCommand(route: RouteMetricRequest) {
|
||||||
route.directions.forEachIndexed { idx, direction ->
|
route.directions.forEachIndexed { idx, direction ->
|
||||||
when (direction) {
|
when (direction) {
|
||||||
FORWARD -> {
|
FORWARD -> {
|
||||||
carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
|
carX += (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
|
||||||
carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
|
carY += (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
|
||||||
}
|
}
|
||||||
BACKWARD -> {
|
BACKWARD -> {
|
||||||
carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
|
carX -= (Math.cos(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
|
||||||
carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
|
carY -= (Math.sin(degreesToRadian(carAngle.toInt())) * route.distances[idx]).toInt()
|
||||||
}
|
}
|
||||||
LEFT -> {
|
LEFT -> {
|
||||||
carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt()
|
carAngle += route.distances[idx]
|
||||||
}
|
}
|
||||||
RIGHT -> {
|
RIGHT -> {
|
||||||
carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt()
|
carAngle -= route.distances[idx]
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -12,6 +12,7 @@ import io.netty.channel.SimpleChannelInboundHandler
|
|||||||
import io.netty.handler.codec.http.DefaultHttpContent
|
import io.netty.handler.codec.http.DefaultHttpContent
|
||||||
import io.netty.util.AttributeKey
|
import io.netty.util.AttributeKey
|
||||||
import objects.Environment
|
import objects.Environment
|
||||||
|
import setRouteMetricUrl
|
||||||
import setRouteUrl
|
import setRouteUrl
|
||||||
import sonarUrl
|
import sonarUrl
|
||||||
import java.util.*
|
import java.util.*
|
||||||
@@ -45,7 +46,7 @@ class ClientHandler : SimpleChannelInboundHandler<Any> {
|
|||||||
val angles = ctx.channel().attr(AttributeKey.valueOf<IntArray>("angles")).get()
|
val angles = ctx.channel().attr(AttributeKey.valueOf<IntArray>("angles")).get()
|
||||||
handlerSonarResponse(response, angles)
|
handlerSonarResponse(response, angles)
|
||||||
}
|
}
|
||||||
setRouteUrl -> {
|
setRouteUrl, setRouteMetricUrl -> {
|
||||||
try {
|
try {
|
||||||
DebugClInterface.exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
|
DebugClInterface.exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
|
||||||
} catch (e: InterruptedException) {
|
} catch (e: InterruptedException) {
|
||||||
|
|||||||
Reference in New Issue
Block a user