alg

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
             }

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 {

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) {

server/src/main/java/algorithm/AbstractAlgorithm.kt

@@ -21,9 +21,11 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
     protected val LEFT = 2
     protected val RIGHT = 3
 
-    private var prevState:CarState? = null
+    private var prevState: CarState? = null
 
     private var prevSonarDistances = mapOf<Int, Double>()
+    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<IntA
             anglesDistances.put(angles[i], distances[i])
         }
 
-        val state = getCarState(anglesDistances)
+        this.requiredAngles = defaultAngles
 
+        val state = getCarState(anglesDistances)
+        if (state == null) {
+            return
+        }
         val command = getCommand(anglesDistances, state)
-        calculateCarPosition(command, state)
         println(Arrays.toString(command.directions))
         println(Arrays.toString(command.times))
 
@@ -126,10 +131,12 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         return prevSonarDistances
     }
 
-    protected abstract fun calculateCarPosition(route: RouteRequest, state: CarState)
+    private fun getAngles(): IntArray {
-    protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState
+        return requiredAngles
+    }
+
+    protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState?
     protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest
-    protected abstract fun getAngles(): IntArray
 
 
     private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest {

server/src/main/java/algorithm/RoomBypassingAlgorithm.kt

@@ -14,17 +14,17 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
 
     public var wallAngleWithOX = 0.0//in radian
     public var wallLength = 0.0//sm
-    var xPos = 0.0
+    var errorCount = 0
-    var yPos = 0.0
+
-    var carAngle = 0.0
 
     private var prevPoint = Pair(0.0, 0.0)
 
-    override fun getCarState(anglesDistances: Map<Int, Double>): CarState {
+    override fun getCarState(anglesDistances: Map<Int, Double>): 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<IntArray>) : 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<Int, Double>, state: CarState): RouteRequest {
         val dist0 = anglesDistances[0]
         val dist60 = anglesDistances[60]
@@ -76,6 +49,13 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : 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<IntArray>) : 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<IntArray>) : 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<IntArray>) : 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<IntArray>) : 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