diff --git a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt index 679ce1f9a5b..ab5e26f43f0 100644 --- a/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt +++ b/car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt @@ -123,7 +123,7 @@ class ControllerEmulator : Controller { continue } val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0) - + Math.pow(yIntersection - ySensor0, 2.0)) * 1000000); + + Math.pow(yIntersection - ySensor0, 2.0)) * 1000000) if (currentDistance < result) { result = currentDistance } diff --git a/server/build.gradle b/server/build.gradle index a37ad67c680..6faf3fde5fd 100644 --- a/server/build.gradle +++ b/server/build.gradle @@ -30,8 +30,14 @@ task getDeps(type: Copy) { from sourceSets.main.compileClasspath into 'build/libs' } +task compileProto() { + exec { + executable './../proto/protofiles_sources/compile_proto.sh' + } +} build.dependsOn getDeps +build.dependsOn compileProto jar { manifest { diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index e4aac6d71d2..2b180c7a95f 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -22,6 +22,8 @@ object DebugClInterface { "refloc - refresh all car locations\n" + "sonar [car_id] - get sonar data\n" + "dbinfo [car_id] [type] - refresh all car locations\n." + + "lines - print lines, detected by car\n" + + "alg [count] - run algorithm. Make [count] iteration\n" + "type is string name of value or int value. available values: MEMORYSTATS - 0\n" + "stop - exit from this interface and stop all threads\n" @@ -57,7 +59,7 @@ object DebugClInterface { "refloc" -> executeRefreshLocationCommand() "sonar" -> executeSonarCommand(readString) "dbinfo" -> executeDebugInfoCommand(readString) - "points" -> println(algorithm.RoomModel.lines) + "lines" -> println(algorithm.RoomModel.lines) "pos" -> { val tmp = algorithmImpl if (tmp is RoomBypassingAlgorithm) { diff --git a/server/src/main/java/algorithm/AbstractAlgorithm.kt b/server/src/main/java/algorithm/AbstractAlgorithm.kt index 5416aff28d9..aef7933fb40 100644 --- a/server/src/main/java/algorithm/AbstractAlgorithm.kt +++ b/server/src/main/java/algorithm/AbstractAlgorithm.kt @@ -21,9 +21,11 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger() + private val defaultAngles = listOf(0, 60, 90, 120, 180).toIntArray() + protected var requiredAngles = defaultAngles protected enum class CarState { WALL, @@ -104,10 +106,13 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger): CarState + private fun getAngles(): IntArray { + return requiredAngles + } + + protected abstract fun getCarState(anglesDistances: Map): CarState? protected abstract fun getCommand(anglesDistances: Map, state: CarState): RouteRequest - protected abstract fun getAngles(): IntArray private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest { diff --git a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt index 60aaaeef7e5..d67c0eb523e 100644 --- a/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt +++ b/server/src/main/java/algorithm/RoomBypassingAlgorithm.kt @@ -14,17 +14,17 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs 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 + var errorCount = 0 + private var prevPoint = Pair(0.0, 0.0) - override fun getCarState(anglesDistances: Map): CarState { + override fun getCarState(anglesDistances: Map): CarState? { 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,... + requiredAngles = getIntArray(0, 60, 90, 120, 180) return CarState.OUTER } if (dist90 < 20) { @@ -40,33 +40,6 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs return args } - override fun getAngles(): IntArray { - 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, state: CarState): RouteRequest { val dist0 = anglesDistances[0] val dist60 = anglesDistances[60] @@ -76,6 +49,13 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0)) if (dist120 == null || dist90 == null || dist60 == null) { println("null distance!") + if (errorCount >= 3) { + errorCount = 0 + resultBuilder.setDirections(getIntArray(BACKWARD)) + resultBuilder.setTimes(getIntArray((15.0 / MOVE_VELOCITY).toInt())) + return resultBuilder.build() + } + errorCount++ return resultBuilder.build() } if (getPrevState() == null) { @@ -107,7 +87,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs } resultBuilder.setDirections(getIntArray(FORWARD)) - resultBuilder.setTimes(getIntArray((25.0 / MOVE_VELOCITY).toInt())) + resultBuilder.setTimes(getIntArray((35.0 / MOVE_VELOCITY).toInt())) return resultBuilder.build() } CarState.INNER -> { @@ -127,6 +107,10 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair())) } } + if (pointsNextLine.size < 3) { + requiredAngles = IntArray(37, { it * 5 }) + return resultBuilder.build() + } val nextLine = approximatePointsByLine(pointsNextLine.toTypedArray()) val currentLine = RoomModel.lines.last() @@ -147,7 +131,9 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val vectorCurrentLine = Vector(xIntersection, yIntersection, prevPoint.first, prevPoint.second) val scalarProduct = vectorCurrentLine.scalarProduct(vectorNextLine) - val angle = Math.acos(scalarProduct / (vectorCurrentLine.length() * vectorNextLine.length())) +// val angle = Math.acos(scalarProduct / (vectorCurrentLine.length() * vectorNextLine.length())) + //todo + val angle = Math.PI / 2 val wallsAngleInDegrees = radiansToDegrees(angle) wallAngleWithOX += degreesToRadian(180 - wallsAngleInDegrees.toInt()) prevPoint = Pair(xIntersection, yIntersection) @@ -197,6 +183,10 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger) : Abs val sumXY = points.sumByDouble { it.second * it.first } val pointsCount = points.size + val den = pointsCount * sumXQuad - sumX * sumX + if (Math.abs(den) < 0.001) { + return Line(1.0, 0.0, -points.first().first) + } val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX) val b = (sumY - sumX * k) / pointsCount