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

car emulator, processing sonar request

Browse Source
This commit is contained in:
MaximZaitsev
2016-08-22 13:05:57 +03:00
parent 0827ec0c3b
commit 612acd4667
5 changed files with 80 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
car_srv/kotlinSrv/src/JSImport.kt
+6 -1
View File
@@ -22,7 +22,7 @@ fun <T> encodeProtoBuf(protoMessage: T): ByteArray {
routeBytes = ByteArray(protoSize)
val codedOutput = CodedOutputStream(routeBytes)
protoMessage.writeTo(codedOutput)
}else if (protoMessage is DebugRequest) {
} else if (protoMessage is DebugRequest) {
val protoSize = protoMessage.getSizeNoTag()
routeBytes = ByteArray(protoSize)
val codedOutput = CodedOutputStream(routeBytes)
@@ -52,6 +52,11 @@ fun <T> encodeProtoBuf(protoMessage: T): ByteArray {
routeBytes = ByteArray(protoSize)
val codedOutput = CodedOutputStream(routeBytes)
protoMessage.writeTo(codedOutput)
} else if (protoMessage is SonarResponse) {
val protoSize = protoMessage.getSizeNoTag()
routeBytes = ByteArray(protoSize)
val codedOutput = CodedOutputStream(routeBytes)
protoMessage.writeTo(codedOutput)
} else {
println("PROTO MESSAGE DON'T ENCODE!")
routeBytes = ByteArray(0)
car_srv/kotlinSrv/src/Main.kt
+2 -2
View File
@@ -4,7 +4,7 @@ import net.server.handlers.AbstractHandler
import net.server.handlers.debug.Memory
import net.server.handlers.flash.LoadBin
import net.server.handlers.main.GetLocation
import net.server.handlers.main.SetRoute
import net.server.handlers.main.GetSonarData
import net.server.handlers.rc.Control
val carServerPort: Int = 8888
@@ -23,7 +23,7 @@ fun main(args: Array<String>) {
val carController = ControllerToUsb()
handlers.put("/rc/control", Control())
handlers.put("/loadBin", LoadBin())
handlers.put("/route", SetRoute(carController))
handlers.put("/sonar", GetSonarData(carController))
handlers.put("/getLocation", GetLocation())
handlers.put("/debug/memory", Memory())
car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt
+36 -12
View File
@@ -3,6 +3,7 @@ package control.emulator
import CarState
import RouteRequest
import RouteResponse
import SonarResponse
import control.Controller
import encodeProtoBuf
import room.Data
@@ -46,8 +47,6 @@ class ControllerEmulator : Controller {
override fun executeRequestSensorData(angles: IntArray, callBack: (ByteArray) -> Unit) {
//calculate distance
val xSensor0 = CarState.instance.x
val ySensor0 = CarState.instance.y
val carAngle = CarState.instance.angle
@@ -55,10 +54,9 @@ class ControllerEmulator : Controller {
val distances = arrayListOf<Int>()
angles.forEach { angle ->
val angleFinal = ((carAngle - angle) % 360)
//tg can be equal to inf if angle = 90. it vertical line and x1 = x0, y1 = y0+[any number. eg 1]
val xSensor1: Int
val ySensor1: Double
//tg can be equal to inf if angle = 90. it vertical line. x1 = x0, y1 = y0+[any number. eg 1]
if (angleFinal == 90) {
xSensor1 = xSensor0
ySensor1 = (ySensor0 + 1).toDouble()
@@ -68,6 +66,7 @@ class ControllerEmulator : Controller {
}
val sensorLine = Line(ySensor0 - ySensor1, xSensor1.toDouble() - xSensor0, xSensor0 * ySensor1 - ySensor0 * xSensor1)
var distance = Int.MAX_VALUE
for (wall in Data.walls) {
val wallLine = wall.line
val coef = sensorLine.A * wallLine.B - sensorLine.B * wallLine.A
@@ -80,7 +79,8 @@ class ControllerEmulator : Controller {
val xIntersection = (sensorLine.B * wallLine.C - wallLine.B * sensorLine.C) / coef
val yIntersection = (sensorLine.C * wallLine.A - wallLine.C * sensorLine.A) / coef
//filter by direction
//filters by direction and intersection position
if (angle > 90 && angle < 270) {
//negative direction for OX
if (xIntersection >= xSensor0) {
@@ -91,13 +91,35 @@ class ControllerEmulator : Controller {
continue
}
}
if (Math.min(wall.xTo, wall.xFrom) > xIntersection
|| Math.max(wall.xTo, wall.xFrom) < xIntersection
|| Math.min(wall.yFrom, wall.yTo) > yIntersection
|| Math.max(wall.yFrom, wall.yTo) < yIntersection) {
continue
}
val currentDistance = Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0)
+ Math.pow(yIntersection - ySensor0, 2.0)).toInt()
if (currentDistance < distance) {
distance = currentDistance
}
}
//Math.random() return double in [0,1)
val delta = getRandomIntFrom(IntArray(5, { x -> x - 2 }))//return one of -2 -1 0 1 or 2
distances.add(distance + delta)
}
val responseMessage = SonarResponse.BuilderSonarResponse(distances.toIntArray())
val bytesMessage = encodeProtoBuf(responseMessage)
callBack.invoke(bytesMessage)
}
//return random int from array
private fun getRandomIntFrom(values: IntArray): Int {
val randomValue = (Math.random() * 1000).toInt()//value in [0,999]
val randomArrayIdx = randomValue * values.size / 1000//index in [0, values.size-1]
return values[randomArrayIdx]
}
fun executeCommand(commands: List<Pair<MoveDirection, Int>>, currentCommandIdx: Int, callBack: (ByteArray) -> Unit) {
if (currentCommandIdx == commands.size) {
val responseMessage = RouteResponse.BuilderRouteResponse(0).build()
@@ -108,11 +130,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 commandTimeIncludeRandom = (commandTime * delta).toInt()
when (currentCommand.first) {
MoveDirection.FORWARD -> carInstance.moving((commandTime * MOVE_VELOCITY).toInt() / 1000)
MoveDirection.BACKWARD -> carInstance.moving(-(commandTime * MOVE_VELOCITY).toInt() / 1000)
MoveDirection.RIGHT -> carInstance.rotate((commandTime * ROTATION_VELOCITY).toInt() / 1000)
MoveDirection.LEFT -> carInstance.rotate(-(commandTime * ROTATION_VELOCITY).toInt() / 1000)
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)
else -> {
}
}
car_srv/kotlinSrv/src/net/server/handlers/main/GetSonarData.kt
+35
View File
@@ -0,0 +1,35 @@
package net.server.handlers.main
import CodedInputStream
import McState
import SonarRequest
import SonarResponse
import control.Controller
import encodeProtoBuf
import net.server.handlers.AbstractHandler
class GetSonarData : AbstractHandler {
val fromServerObjectBuilder: SonarRequest.BuilderSonarRequest
val toServerObjectBuilder: SonarResponse.BuilderSonarResponse
val controller: Controller
constructor(routeExecutor: Controller) : super() {
this.controller = routeExecutor
this.fromServerObjectBuilder = SonarRequest.BuilderSonarRequest(IntArray(0))
this.toServerObjectBuilder = SonarResponse.BuilderSonarResponse(IntArray(0))
}
override fun getBytesResponse(data: ByteArray, callback: (ByteArray) -> Unit) {
val message = fromServerObjectBuilder.build()
message.mergeFrom(CodedInputStream(data))
if (!McState.instance.isConnected()) {
println("mc is disconnected!")
val responseMessage = toServerObjectBuilder.build()
callback.invoke(encodeProtoBuf(responseMessage))
return
}
controller.executeRequestSensorData(message.angles, callback)
}
}
car_srv/kotlinSrv/src/room/Wall.kt
+1 -1
View File
@@ -1,5 +1,5 @@
package room
class Wall constructor(val line: Line, val xFrom: Int, val xTo: Int, yFrom: Int, yTo: Int) {
class Wall constructor(val line: Line, val xFrom: Int, val xTo: Int, val yFrom: Int, val yTo: Int) {
}